From 6c0763c7bc55dc714c055492e63b281eb5284ac3 Mon Sep 17 00:00:00 2001 From: D-NXKT Date: Mon, 10 Mar 2014 23:02:16 +0100 Subject: [PATCH] winch/aerotowing support for JSBSim-aircrafts; visible tow- and winch-ropes for YASim and JSBSim-planes --- Nasal/towing/hitch.nas | 1663 ++++++++++++++++++++++++++++++++++++++++ 1 file changed, 1663 insertions(+) create mode 100644 Nasal/towing/hitch.nas diff --git a/Nasal/towing/hitch.nas b/Nasal/towing/hitch.nas new file mode 100644 index 000000000..a4c0e87d8 --- /dev/null +++ b/Nasal/towing/hitch.nas @@ -0,0 +1,1663 @@ +# +# Version: 10. March 2014 +# +# Purpose of this routine: +# ------------------------ +# +# - Create visible winch- and towropes for gliders and towplanes +# - Support of aerotowing and winch for JSBSim-aircrafts (glider and towplanes) +# +# This routine is very similar to /FDM/YASim/Hitch.cpp +# Aerotowing is fully compatible to the YASim functionality. +# This means that YASim-gliders could be towed by JSBSim-aircrafts and vice versa. +# Setup-instructions with copy and paste examples are given below: +# +# +# Setup of visible winch/towropes for Yasim-aircrafts: +# ---------------------------------------------------- +# +# YASim-aircrafts with winch/aerotowing functionality should work out of the box. +# Optional you can customize the rope-diameter by adding the following to "your_aircraft-set.xml": +# +# +# +# +# 10 +# +# +# +# +# 20 +# +# +# +# +# +# That's all! +# +# +# +# Support of aerotowing and winch for JSBSim-aircrafts (glider and towplanes): +# ---------------------------------------------------------------------------- +# +# 1. Define a hitch in the JSBSim-File. Coordinates according to JSBSims structural frame of reference +# (x points to the tail, y points to the right wing, z points upwards). Be careful! This +# coordinates don't appear in the property tree. You can only check them with test flights! +# The visible towrope is NOT an indicator of correct settings! +# Unit must be "LBS", frame must be "BODY". The force name is arbitrary but MUST end with "_x", "_y","_z", respectively. +# +# +# +# +# 3.65 +# 0.0 +# -0.12 +# +# +# 1.0 +# 0.0 +# 0.0 +# +# +# +# +# +# 3.65 +# 0.0 +# -0.12 +# +# +# 0.0 +# 1.0 +# 0.0 +# +# +# +# +# +# 3.65 +# 0.0 +# -0.12 +# +# +# 0.0 +# 0.0 +# 1.0 +# +# +# + +# 2. Define controls for aerotowing and winch. +# Add the following key bindings in "yourAircraft-set.xml": +# +# +# +# +# Ctrl-o +# Find aircraft for aerotow +# +# nasal +# +# +# +# +# +# o +# Lock aerotow-hook +# +# nasal +# +# +# +# +# +# O +# Open aerotow-hook +# +# nasal +# +# +# +# +# +# Ctrl-w +# Place Winch and hook in +# +# nasal +# +# +# +# +# +# w +# Start winch +# +# nasal +# +# +# +# +# +# W +# Open winch-hook +# +# nasal +# +# +# +# +# +# +# +# For towplanes only "key n=79" (Open aerotow-hook) is required! +# +# +# 3. Set mandatory properties: +# +# +# +# hitch +# 1.5 +# 0.00 +# -0.3 +# false +# 0.0 +# +# +# hitch +# 0.0 +# 0.0 +# 0.0 +# +# +# +# +# "basename_force_jsbsim" must be the external force name in JSBSim but without the ending "_x", "_y", "_z". +# +# IMPORTANT: +# JSBSim doesn't provide the hitch coordinates in the property tree. Hence you must set them again to get a +# visible towrope (local-pos-x/y/z). Unfortunately the coordinate systems are different. Here the coordinates +# for the "YASim-System" are needed (x points to the nose, y points to the left wing, z points upwards). +# If you see the rope at the expected position "local-pos-x/y/z" are correct. +# +# "force-is-calculated-by-other" should be "false" for gliders and "true" for towplanes. +# "mp-auto-connect-period" is only needed for towplanes and should be "1". + + +# 4. Set optional properties: +# +# +# +# +# 6000 +# 9000 +# +# +# 20 +# +# +# +# 70. +# +# 1000. +# 1500. +# 800. +# 100. +# 15. +# 20. +# +# +# +# 10000 +# 40000 +# +# +# 40 +# +# +# +# +# +# That's it! + + +################################################## general info ############################################ +# +# 3 different types of towplanes could exist: AI-plane, MP-plane without interaction, MP-plane with interaction. +# AI-planes are identified by the node "ai/models/aircraft/". +# MP-planes (interactice/non-interactive) are identified by the existence of node "ai/models/multiplayer". +# Interactive MP-plane: variables in node "ai/models/multiplayer/sim/hitches/" are updated. +# Non-interactive MP-plane: variables are not updated (values are either not defined or have "wrong" values +# from a former owner of this node. +# +# The following properties are transmitted in multiplayer: +# "sim/hitches/aerotow/tow/elastic-constant" +# "sim/hitches/aerotow/tow/weight-per-m-kg-m" +# "sim/hitches/aerotow/tow/dist" +# "sim/hitches/aerotow/tow/connected-to-property-node" +# "sim/hitches/aerotow/tow/connected-to-ai-or-mp-callsign" +# "sim/hitches/aerotow/tow/brake-force" +# "sim/hitches/aerotow/tow/end-force-x" +# "sim/hitches/aerotow/tow/end-force-y" +# "sim/hitches/aerotow/tow/end-force-z" +# "sim/hitches/aerotow/is-slave" +# "sim/hitches/aerotow/speed-in-tow-direction" +# "sim/hitches/aerotow/open", open); +# "sim/hitches/aerotow/local-pos-x" +# "sim/hitches/aerotow/local-pos-y" +# "sim/hitches/aerotow/local-pos-z" +# +############################################################################################################## + + + + +# ###################################################################################################################### +# check, if towing support makes sense +# ###################################################################################################################### + +# Check if node "sim/hitches" is defined. If not, return! + if (props.globals.getNode("sim/hitches") == nil ) return; + print("towing is active!"); + + +# ###################################################################################################################### +# set defaults / initialize at startup +# ###################################################################################################################### + +# set defaults for properties that are NOT already defined + + # yasim properties for aerotow (should be already defined for yasim aircrafts but not for JSBSim aircrafts + if (props.globals.getNode("sim/hitches/aerotow/broken") == nil ) + props.globals.getNode("sim/hitches/aerotow/broken", 1).setBoolValue(0); + if (props.globals.getNode("sim/hitches/aerotow/force") == nil ) + props.globals.getNode("sim/hitches/aerotow/force", 1).setValue(0.); + if (props.globals.getNode("sim/hitches/aerotow/force-is-calculated-by-other") == nil ) + props.globals.getNode("sim/hitches/aerotow/force-is-calculated-by-other", 1).setBoolValue(0); + if (props.globals.getNode("sim/hitches/aerotow/is-slave") == nil ) + props.globals.getNode("sim/hitches/aerotow/is-slave", 1).setBoolValue(0); + if (props.globals.getNode("sim/hitches/aerotow/local-pos-x") == nil ) + props.globals.getNode("sim/hitches/aerotow/local-pos-x", 1).setValue(0.); + if (props.globals.getNode("sim/hitches/aerotow/local-pos-y") == nil ) + props.globals.getNode("sim/hitches/aerotow/local-pos-y", 1).setValue(0.); + if (props.globals.getNode("sim/hitches/aerotow/local-pos-z") == nil ) + props.globals.getNode("sim/hitches/aerotow/local-pos-z", 1).setValue(0.); + if (props.globals.getNode("sim/hitches/aerotow/mp-auto-connect-period") == nil ) + props.globals.getNode("sim/hitches/aerotow/mp-auto-connect-period", 1).setValue(0.); + if (props.globals.getNode("sim/hitches/aerotow/mp-time-lag") == nil ) + props.globals.getNode("sim/hitches/aerotow/mp-time-lag", 1).setValue(0.); + #if (props.globals.getNode("sim/hitches/aerotow/open") == nil ) + props.globals.getNode("sim/hitches/aerotow/open", 1).setBoolValue(1); + if (props.globals.getNode("sim/hitches/aerotow/speed-in-tow-direction") == nil ) + props.globals.getNode("sim/hitches/aerotow/speed-in-tow-direction", 1).setValue(0.); + if (props.globals.getNode("sim/hitches/aerotow/tow/brake-force") == nil ) + props.globals.getNode("sim/hitches/aerotow/tow/brake-force", 1).setValue(12345.); + if (props.globals.getNode("sim/hitches/aerotow/tow/connected-to-ai-node") == nil ) + props.globals.getNode("sim/hitches/aerotow/tow/connected-to-ai-node", 1).setBoolValue(0); + if (props.globals.getNode("sim/hitches/aerotow/tow/connected-to-ai-or-mp-callsign") == nil ) + props.globals.getNode("sim/hitches/aerotow/tow/connected-to-ai-or-mp-callsign", 1).setValue(""); + if (props.globals.getNode("sim/hitches/aerotow/tow/connected-to-ai-or-mp-id") == nil ) + props.globals.getNode("sim/hitches/aerotow/tow/connected-to-ai-or-mp-id", 1).setIntValue(0); + if (props.globals.getNode("sim/hitches/aerotow/tow/connected-to-mp-node") == nil ) + props.globals.getNode("sim/hitches/aerotow/tow/connected-to-mp-node", 1).setBoolValue(0); + if (props.globals.getNode("sim/hitches/aerotow/tow/connected-to-property-node") == nil ) + props.globals.getNode("sim/hitches/aerotow/tow/connected-to-property-node", 1).setBoolValue(0); + if (props.globals.getNode("sim/hitches/aerotow/tow/dist") == nil ) + props.globals.getNode("sim/hitches/aerotow/tow/dist", 1).setValue(0.); + if (props.globals.getNode("sim/hitches/aerotow/tow/elastic-constant") == nil ) + props.globals.getNode("sim/hitches/aerotow/tow/elastic-constant", 1).setValue(9111.); + if (props.globals.getNode("sim/hitches/aerotow/tow/end-force-x") == nil ) + props.globals.getNode("sim/hitches/aerotow/tow/end-force-x", 1).setValue(0.); + if (props.globals.getNode("sim/hitches/aerotow/tow/end-force-y") == nil ) + props.globals.getNode("sim/hitches/aerotow/tow/end-force-y", 1).setValue(0.); + if (props.globals.getNode("sim/hitches/aerotow/tow/end-force-z") == nil ) + props.globals.getNode("sim/hitches/aerotow/tow/end-force-z", 1).setValue(0.); + if (props.globals.getNode("sim/hitches/aerotow/tow/length") == nil ) + props.globals.getNode("sim/hitches/aerotow/tow/length", 1).setValue(60.); + if (props.globals.getNode("sim/hitches/aerotow/tow/node") == nil ) + props.globals.getNode("sim/hitches/aerotow/tow/node", 1).setValue(""); + if (props.globals.getNode("sim/hitches/aerotow/tow/weight-per-m-kg-m") == nil ) + props.globals.getNode("sim/hitches/aerotow/tow/weight-per-m-kg-m", 1).setValue(0.35); + + # additional properties + if (props.globals.getNode("sim/hitches/aerotow/oldOpen") == nil ) + props.globals.getNode("sim/hitches/aerotow/oldOpen", 1).setBoolValue(1); + + # new properties for towrope + if (props.globals.getNode("sim/hitches/aerotow/rope/exist") == nil ) + props.globals.getNode("sim/hitches/aerotow/rope/exist", 1).setBoolValue(0); + if (props.globals.getNode("sim/hitches/aerotow/rope/model_id") == nil ) + props.globals.getNode("sim/hitches/aerotow/rope/model_id", 1).setIntValue(-1); + if (props.globals.getNode("sim/hitches/aerotow/rope/path_to_model") == nil ) + props.globals.getNode("sim/hitches/aerotow/rope/path_to_model", 1).setValue("Models/Aircraft/towropes.xml"); + if (props.globals.getNode("sim/hitches/aerotow/rope/rope-diameter-mm") == nil ) + props.globals.getNode("sim/hitches/aerotow/rope/rope-diameter-mm", 1).setIntValue(20.); + + # new properties for JSBSim aerotow + if ( getprop("sim/flight-model") == "jsb" ) { + if (props.globals.getNode("sim/hitches/aerotow/basename_force_jsbsim") == nil ) + props.globals.getNode("sim/hitches/aerotow/basename_force_jsbsim", 1).setValue("hitch"); + if (props.globals.getNode("sim/hitches/aerotow/mp_oldOpen") == nil ) + props.globals.getNode("sim/hitches/aerotow/mp_oldOpen", 1).setBoolValue(1); + if (props.globals.getNode("sim/hitches/aerotow/tow/mp_last_reporded_dist") == nil ) + props.globals.getNode("sim/hitches/aerotow/tow/mp_last_reported_dist", 1).setValue(0.); + } + + # yasim properties for winch (should be already defined for yasim aircrafts but not for JSBSim aircrafts + #if (props.globals.getNode("sim/hitches/winch/open") == nil ) + props.globals.getNode("sim/hitches/winch/open", 1).setBoolValue(1); + if (props.globals.getNode("sim/hitches/winch/broken") == nil ) + props.globals.getNode("sim/hitches/winch/broken", 1).setBoolValue(0); + if (props.globals.getNode("sim/hitches/winch/winch/global-pos-x") == nil ) + props.globals.getNode("sim/hitches/winch/winch/global-pos-x", 1).setValue(0.); + if (props.globals.getNode("sim/hitches/winch/winch/global-pos-y") == nil ) + props.globals.getNode("sim/hitches/winch/winch/global-pos-y", 1).setValue(0.); + if (props.globals.getNode("sim/hitches/winch/winch/global-pos-z") == nil ) + props.globals.getNode("sim/hitches/winch/winch/global-pos-z", 1).setValue(0.); + if (props.globals.getNode("sim/hitches/winch/winch/initial-tow-length-m") == nil ) + props.globals.getNode("sim/hitches/winch/winch/initial-tow-length-m", 1).setValue(1000.); + if (props.globals.getNode("sim/hitches/winch/winch/max-tow-length-m") == nil ) + props.globals.getNode("sim/hitches/winch/winch/max-tow-length-m", 1).setValue(1500.); + if (props.globals.getNode("sim/hitches/winch/winch/min-tow-length-m") == nil ) + props.globals.getNode("sim/hitches/winch/winch/min-tow-length-m", 1).setValue(1.); + + if (props.globals.getNode("sim/hitches/winch/tow/length") == nil ) + props.globals.getNode("sim/hitches/winch/tow/length", 1).setValue(0.); + if (props.globals.getNode("sim/hitches/winch/tow/dist") == nil ) + props.globals.getNode("sim/hitches/winch/tow/dist", 1).setValue(0.); + if (props.globals.getNode("sim/hitches/winch/tow/elastic-constant") == nil ) + props.globals.getNode("sim/hitches/winch/tow/elastic-constant", 1).setValue(40001.); + if (props.globals.getNode("sim/hitches/winch/tow/weight-per-m-kg-m") == nil ) + props.globals.getNode("sim/hitches/winch/tow/weight-per-m-kg-m", 1).setValue(0.1); + + # additional properties + if (props.globals.getNode("sim/hitches/winch/oldOpen") == nil ) + props.globals.getNode("sim/hitches/winch/oldOpen", 1).setBoolValue(1); + if (props.globals.getNode("sim/hitches/winch/winch/max-spool-speed-m-s") == nil ) + props.globals.getNode("sim/hitches/winch/winch/max-spool-speed-m-s", 1).setValue(40.); + + # new properties for winch-rope + if (props.globals.getNode("sim/hitches/winch/rope/exist") == nil ) + props.globals.getNode("sim/hitches/winch/rope/exist", 1).setBoolValue(0); + if (props.globals.getNode("sim/hitches/winch/rope/model_id") == nil ) + props.globals.getNode("sim/hitches/winch/rope/model_id", 1).setIntValue(-1); + if (props.globals.getNode("sim/hitches/winch/rope/path_to_model") == nil ) + props.globals.getNode("sim/hitches/winch/rope/path_to_model", 1).setValue("Models/Aircraft/towropes.xml"); + if (props.globals.getNode("sim/hitches/winch/rope/rope-diameter-mm") == nil ) + props.globals.getNode("sim/hitches/winch/rope/rope-diameter-mm", 1).setIntValue(20.); + + # new properties for JSBSim winch + if ( getprop("sim/flight-model") == "jsb" ) { + if (props.globals.getNode("sim/hitches/winch/basename_force_jsbsim") == nil ) + props.globals.getNode("sim/hitches/winch/basename_force_jsbsim", 1).setValue("hitch"); + if (props.globals.getNode("sim/hitches/winch/automatic-release-angle-deg") == nil ) + props.globals.getNode("sim/hitches/winch/automatic-release-angle-deg", 1).setValue(361.); + if (props.globals.getNode("sim/hitches/winch/winch/clutched") == nil ) + props.globals.getNode("sim/hitches/winch/winch/clutched", 1).setBoolValue(0); + if (props.globals.getNode("sim/hitches/winch/winch/actual-spool-speed-m-s") == nil ) + props.globals.getNode("sim/hitches/winch/winch/actual-spool-speed-m-s", 1).setValue(0.); + if (props.globals.getNode("sim/hitches/winch/winch/spool-acceleration-m-s-s") == nil ) + props.globals.getNode("sim/hitches/winch/winch/spool-acceleration-m-s-s", 1).setValue(8.); + if (props.globals.getNode("sim/hitches/winch/winch/max-unspool-speed-m-s") == nil ) + props.globals.getNode("sim/hitches/winch/winch/max-unspool-speed-m-s", 1).setValue(40.); + if (props.globals.getNode("sim/hitches/winch/winch/actual-force-N") == nil ) + props.globals.getNode("sim/hitches/winch/winch/actual-force-N", 1).setValue(0.); + if (props.globals.getNode("sim/hitches/winch/winch/max-force-N") == nil ) + props.globals.getNode("sim/hitches/winch/winch/max-force-N", 1).setValue(1000.); + if (props.globals.getNode("sim/hitches/winch/winch/max-power-kW") == nil ) + props.globals.getNode("sim/hitches/winch/winch/max-power-kW", 1).setValue(123.); + if (props.globals.getNode("sim/hitches/winch/tow/break-force-N") == nil ) + props.globals.getNode("sim/hitches/winch/tow/break-force-N", 1).setValue(12345.); + if (props.globals.getNode("sim/hitches/winch/winch/magic-constant") == nil ) + props.globals.getNode("sim/hitches/winch/winch/magic-constant", 1).setValue(500.); + } + + + +# ###################################################################################################################### +# main function +# ###################################################################################################################### + +var towing = func { + + #print("function towing is running"); + + var FT2M = 0.30480; + var M2FT = 1 / FT2M; + var dt = 0; + + # ------------------------------- aerotow part ------------------------------- + + var open = getprop("sim/hitches/aerotow/open"); + var oldOpen = getprop("sim/hitches/aerotow/oldOpen"); + + if ( open != oldOpen ) { # check if my hitch state has changed, if yes: message + #print("state has changed: open=",open," oldOpen=",oldOpen); + + if ( !open ) { # my hitch was open and is closed now + if ( getprop("sim/flight-model") == "jsb" ) { + var distance = getprop("sim/hitches/aerotow/tow/dist"); + var towlength_m = getprop("sim/hitches/aerotow/tow/length"); + if ( distance > towlength_m * 1.0001 ) { + setprop("sim/messages/pilot", sprintf("Could not lock hitch (tow length is insufficient) on hitch %i!", + getprop("sim/hitches/aerotow/tow/connected-to-mp-node"))); + props.globals.getNode("sim/hitches/aerotow/open").setBoolValue(1); # open my hitch again + } # mp aircraft to far away + else { # my hitch is closed + setprop("sim/messages/pilot", sprintf("Locked hitch aerotow %i!", + getprop("sim/hitches/aerotow/tow/connected-to-mp-node"))); + } + props.globals.getNode("sim/hitches/aerotow/broken").setBoolValue(0); + } # end: JSBSim + if ( !getprop("sim/hitches/aerotow/open") ) { + # setup ai-towrope + createTowrope("aerotow"); + + # set default hitch coordinates (needed for Ai- and non-interactive MP aircrafts) + setAIObjectDefaults() ; + } + } # end hitch is closed + + if ( open ) { # my hitch is now open + if ( getprop("sim/flight-model") == "jsb" ) { + if ( getprop("sim/hitches/aerotow/broken") ) { + setprop("sim/messages/pilot", sprintf("Oh no, the tow is broken")); + } + else { + setprop("sim/messages/pilot", sprintf("Opened hitch aerotow %i!", + getprop("sim/hitches/aerotow/tow/connected-to-mp-node"))); + } + releaseHitch("aerotow"); # open=1 / forces=0 + } # end: JSBSim + removeTowrope("aerotow"); # remove towrope model + } # end hitch is open + + setprop("sim/hitches/aerotow/oldOpen",open); + } # end hitch state has changed + + if (!open ) { + aerotow(open); + } # end hitch is closed (open == 0) + else { # my hitch is open + var mp_auto_connect_period = props.globals.getNode("sim/hitches/aerotow/mp-auto-connect-period").getValue(); + if ( mp_auto_connect_period != 0 ) { # if auto-connect + if ( getprop("sim/flight-model") == "jsb" ) { # only for JSBSim aircraft + findBestAIObject(); + } # end JSBSim aircraft + dt = mp_auto_connect_period; + #print("towing: running as auto connect with period=",dt); + } # end if auto-connect + else { # my hitch is open and not auto-connect + dt = 0; + } + } + + + # ------------------------------- winch part ------------------------------- + + var winchopen = getprop("sim/hitches/winch/open"); + var wincholdOpen = getprop("sim/hitches/winch/oldOpen"); + + if ( winchopen != wincholdOpen ) { # check if my hitch state has changed, if yes: message + #print("winch state has changed: open=",winchopen," oldOpen=",wincholdOpen); + if ( !winchopen ) { # my hitch was open and is closed now + if ( getprop("sim/flight-model") == "jsb" ) { + var distance = getprop("sim/hitches/winch/tow/dist"); + var towlength_m = getprop("sim/hitches/winch/tow/length"); + if ( distance > towlength_m ) { + setprop("sim/messages/pilot", sprintf("Could not lock hitch (tow length is insufficient) on hitch %i!", + getprop("sim/hitches/aerotow/tow/connected-to-mp-node"))); + props.globals.getNode("sim/hitches/aerotow/open").setBoolValue(1); # open my hitch again + } # mp aircraft to far away + else { # my hitch is closed + setprop("sim/messages/pilot", sprintf("Locked hitch winch %i!", + getprop("sim/hitches/aerotow/tow/connected-to-mp-node"))); + setprop("sim/hitches/winch/winch/clutched","false"); + } + props.globals.getNode("sim/hitches/winch/broken").setBoolValue(0); + props.globals.getNode("sim/hitches/winch/winch/actual-spool-speed-m-s").setValue(0.); + } # end: JSBSim + if ( !getprop("sim/hitches/winch/open") ) { + # setup ai-towrope + createTowrope("winch"); + + # set default hitch coordinates (needed for Ai- and non-interactive MP aircrafts) + setAIObjectDefaults() ; + } + } # end hitch is closed + + if ( winchopen ) { # my hitch is now open + if ( getprop("sim/flight-model") == "jsb" ) { + if ( getprop("sim/hitches/winch/broken") ) { + setprop("sim/messages/pilot", sprintf("Oh no, the tow is broken")); + } + releaseHitch("winch"); + } # end: JSBSim + pull_in_rope(); + } # end hitch is open + + setprop("sim/hitches/winch/oldOpen",winchopen); + } # end hitch state has changed + + if (!winchopen ) { + winch(winchopen); + } + + settimer( towing, dt ); + +} # end towing + + +# ###################################################################################################################### +# find best AI object +# ###################################################################################################################### + +var findBestAIObject = func (){ + + # the nearest found plane, that is close enough will be used + # set some default variables, needed later to identify if the found object is + # an AI-Object, a "non-interactiv MP-Object or an interactive MP-Object + + # local variables + var aiobjects = []; # keeps the ai-planes from the property tree + var AI_id = 0; # id of towplane + var callsign = 0; # callsign of towplane + var aiPosition = geo.Coord.new(); # current processed ai-plane + var lat_deg = 0; # latitude of current processed aiobject + var lon_deg = 0; # longitude of current processed aiobject + var alt_m = 0; # altitude of current processed aiobject + var myPosition = geo.Coord.new(); # coordinates of glider + var distance_m = 0; # distance to ai-plane + + var FT2M = 0.30480; + + var nodeIsAiAircraft = 0; + var nodeIsMpAircraft = 0; + var running_as_autoconnect = 0; + var mp_open_last_state = 0; + var isSlave = 0; + + if ( getprop("sim/flight-model") == "yasim" ) return; # bypass this routine for Yasim-aircrafts + + #print("findBestAIObject"); + + if (props.globals.getNode("sim/hitches/aerotow/mp-auto-connect-period").getValue() != 0 ) { + var running_as_autoconnect = 1; + #print("findBestAIObject: running as auto connect"); + } + + var towlength_m = props.globals.getNode("sim/hitches/aerotow/tow/length").getValue(); + + var bestdist_m = towlength_m; # initial value + + myPosition = geo.aircraft_position(); + # todo: calculate exact hitch position + + if( running_as_autoconnect ) { + var mycallsign = props.globals.getNode("sim/multiplay/callsign").getValue(); + #print('mycallsign=',mycallsign); + } + + var found = 0; + aiobjects = props.globals.getNode("ai/models").getChildren(); + foreach (var aimember; aiobjects) { + if ( (var node = aimember.getName() ) != nil ) { + nodeIsAiAircraft = 0; + nodeIsMpAircraft = 0; + if ( sprintf("%8s",node) == "aircraft" ) nodeIsAiAircraft = 1; + if ( sprintf("%11s",node) == "multiplayer" ) nodeIsMpAircraft = 1; + #print("found NodeName=",node," nodeIsAiAircraft=",nodeIsAiAircraft," nodeIsMpAircraft=",nodeIsMpAircraft ); + + if( !nodeIsAiAircraft and !nodeIsMpAircraft ) continue; + + if( running_as_autoconnect ) { + if ( !nodeIsMpAircraft ) continue; + if ( aimember.getValue("sim/hitches/aerotow/open") == 1 ) continue; # if mp hook open, auto-connect isn't possible + if (mycallsign != aimember.getValue("sim/hitches/aerotow/tow/connected-to-ai-or-mp-callsign") ) continue ; # I'm the wrong one + } + + var lat_deg = aimember.getNode("position/latitude-deg").getValue(); + var lon_deg = aimember.getNode("position/longitude-deg").getValue(); + var alt_m = aimember.getNode("position/altitude-ft").getValue() * FT2M; + + var aiPosition = geo.Coord.set_latlon( lat_deg, lon_deg, alt_m ); + distance_m = (myPosition.distance_to(aiPosition)); + #print('distance_m=',distance_m,' bestdist_m=',bestdist_m); + if ( distance_m < bestdist_m ) { + bestdist_m = distance_m; + + var towEndNode = node; + var nodeID = aimember.getNode("id").getValue(); + var aicallsign = aimember.getNode("callsign").getValue(); + #print('nodeId=',nodeID,' AiCallsign=',aicallsign); + + #set properties + props.globals.getNode("sim/hitches/aerotow/open").setBoolValue(0); + props.globals.getNode("sim/hitches/aerotow/tow/connected-to-ai-node").setBoolValue(nodeIsAiAircraft); + props.globals.getNode("sim/hitches/aerotow/tow/connected-to-mp-node").setBoolValue(nodeIsMpAircraft); + props.globals.getNode("sim/hitches/aerotow/tow/connected-to-ai-or-mp-callsign").setValue(aicallsign); + props.globals.getNode("sim/hitches/aerotow/tow/connected-to-ai-or-mp-id").setIntValue(nodeID); + props.globals.getNode("sim/hitches/aerotow/tow/connected-to-property-node").setBoolValue(1); + props.globals.getNode("sim/hitches/aerotow/tow/node").setValue(towEndNode); + props.globals.getNode("sim/hitches/aerotow/tow/dist").setValue(bestdist_m); + props.globals.getNode("sim/hitches/aerotow/tow/mp_last_reported_dist", 1).setValue(0.); + + # Set some dummy values. In case of an "interactive"-MP plane + # the correct values will be transmitted in the following loop + aimember.getNode("sim/hitches/aerotow/local-pos-x",1).setValue(-5.); + aimember.getNode("sim/hitches/aerotow/local-pos-y",1).setValue(0.); + aimember.getNode("sim/hitches/aerotow/local-pos-z",1).setValue(0.); + aimember.getNode("sim/hitches/aerotow/tow/dist",1).setValue(-1.); + + found = 1; + } + } + } + if (found) { + if ( !running_as_autoconnect) { + setprop("sim/messages/pilot", sprintf("%s, I am on your hook, distance %4.3f meter.",aicallsign,bestdist_m)); + } + else { + setprop("sim/messages/ai-plane", sprintf("%s: I am on your hook, distance %4.3f meter.",aicallsign,bestdist_m )); + } + if ( running_as_autoconnect ) { + isSlave = 1; + props.globals.getNode("sim/hitches/aerotow/is-slave").setBoolValue(isSlave); + } + # set the dist value to some value below the tow length (if not, the hitch would open the next calc force run + distance_m = towlength_m * 0.5; + props.globals.getNode("sim/hitches/aerotow/mp_oldOpen").setBoolValue(1); + + } # end: if found + else { + if (!running_as_autoconnect) { + setprop("sim/messages/atc", sprintf("Sorry, no aircraft for aerotow!")); + } + } + +} # End function findBestAIObject + + +# ###################################################################################################################### + + +# Start the towing animation ASAP +towing(); + + + +# ###################################################################################################################### +# aerotow function +# ###################################################################################################################### + +var aerotow = func (open){ + + #print("function aerotow is running"); + +# if (!open ) { + + ########################################### my hitch position ############################################ + + myPosition = geo.aircraft_position(); + var my_head_deg = getprop("orientation/heading-deg"); + var my_roll_deg = getprop("orientation/roll-deg"); + var my_pitch_deg = getprop("orientation/pitch-deg"); + + # hook coordinates in Yasim-system (x-> nose / y -> left wing / z -> up) + var x = getprop("sim/hitches/aerotow/local-pos-x"); + var y = getprop("sim/hitches/aerotow/local-pos-y"); + var z = getprop("sim/hitches/aerotow/local-pos-z"); + + var alpha_deg = my_roll_deg * (1.); # roll clockwise (looking in x-direction) := + + var beta_deg = my_pitch_deg * (-1.); # pitch clockwise (looking in y-direction) := - + + # transform hook coordinates + 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.); + + var install_distance_m = Xn[0]; # in front of ref-point of glider + var install_side_m = Xn[1]; + var install_alt_m = Xn[2]; + + var myHitch_pos = myPosition.apply_course_distance( my_head_deg , install_distance_m ); + var myHitch_pos = myPosition.apply_course_distance( my_head_deg - 90. , install_side_m ); + myHitch_pos.set_alt(myPosition.alt() + install_alt_m); + + ########################################### ai hitch position ############################################ + + var aiNodeID = getprop("sim/hitches/aerotow/tow/connected-to-ai-or-mp-id"); # id of former found ai/mp aircraft + #print("aiNodeID=",aiNodeID); + + aiobjects = props.globals.getNode("ai/models").getChildren(); + foreach (var aimember; aiobjects) { + if ( (var c = aimember.getNode("id") ) != nil ) { + var testprop = c.getValue(); + if ( testprop == aiNodeID) { + # get coordinates + var ai_lat = aimember.getNode("position/latitude-deg").getValue(); + var ai_lon = aimember.getNode("position/longitude-deg").getValue(); + var ai_alt = (aimember.getNode("position/altitude-ft").getValue()) * FT2M; + #print("ai_lat,lon,alt",ai_lat,ai_lon,ai_alt); + + var ai_pitch_deg = aimember.getNode("orientation/pitch-deg").getValue(); + var ai_roll_deg = aimember.getNode("orientation/roll-deg").getValue(); + var ai_head_deg = aimember.getNode("orientation/true-heading-deg").getValue(); + + var aiHitchX = aimember.getNode("sim/hitches/aerotow/local-pos-x").getValue(); + var aiHitchY = aimember.getNode("sim/hitches/aerotow/local-pos-y").getValue(); + var aiHitchZ = aimember.getNode("sim/hitches/aerotow/local-pos-z").getValue(); + + if ( getprop("sim/flight-model") == "jsb" ) { + # check if the multiplayer hitch state has changed + # this trick avoids immediate opening after locking because MP-aircraft has not yet reported a locked hitch + if ( (var d = aimember.getNode("sim/hitches/aerotow/open") ) != nil ) { + var mpOpen = aimember.getNode("sim/hitches/aerotow/open").getValue(); + var mp_oldOpen = getprop("sim/hitches/aerotow/mp_oldOpen"); + #print('mpOpen=',mpOpen,' mp_oldOpen=',mp_oldOpen); + if ( mpOpen != mp_oldOpen ) { # state has changed: was open and is now locked OR was locked and is now open + if ( mpOpen ) { + setprop("sim/messages/ai-plane", sprintf("%s: I have released the tow!",getprop("sim/hitches/aerotow/tow/connected-to-ai-or-mp-callsign")) ); + releaseHitch("aerotow"); # my open=1 / forces=0 / remove towrope + } # end: open + props.globals.getNode("sim/hitches/aerotow/mp_oldOpen").setBoolValue(mpOpen); + } # end: state has changed + } # end: node is available + } #end : JSBSim + + var aiPosition = geo.Coord.set_latlon( ai_lat, ai_lon, ai_alt ); + + var alpha_deg = ai_roll_deg * (1.); + var beta_deg = ai_pitch_deg * (-1.); + + # transform hook coordinates + 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.); + + var install_distance_m = Xn[0]; # in front of ref-point of glider + var install_side_m = Xn[1]; + var install_alt_m = Xn[2]; + + var aiHitch_pos = aiPosition.apply_course_distance( ai_head_deg , install_distance_m ); + var aiHitch_pos = aiPosition.apply_course_distance( ai_head_deg - 90. , install_side_m ); + aiHitch_pos.set_alt(aiPosition.alt() + install_alt_m); + + ########################################### distance between hitches ##################################### + + var distance = (myHitch_pos.direct_distance_to(aiHitch_pos)); # distance to plane in meter + var aiHitchheadto = (myHitch_pos.course_to(aiHitch_pos)); + var height = myHitch_pos.alt() - aiHitch_pos.alt(); + + var aiHitchpitchto = -math.asin((myHitch_pos.alt()-aiHitch_pos.alt())/distance) / 0.01745; + #print(" pitch: ", aiHitchpitchto); + + # update position of rope + setprop("ai/models/aerotowrope/position/latitude-deg", myHitch_pos.lat()); + setprop("ai/models/aerotowrope/position/longitude-deg", myHitch_pos.lon()); + setprop("ai/models/aerotowrope/position/altitude-ft", myHitch_pos.alt() * M2FT); + #print("ai_lat,lon,alt",myHitch_pos.lat()," ",myHitch_pos.lon()," ",myHitch_pos.alt() ); + + # update pitch and heading of rope + setprop("ai/models/aerotowrope/orientation/true-heading-deg", aiHitchheadto); + setprop("ai/models/aerotowrope/orientation/pitch-deg", aiHitchpitchto); + + # update length of rope + setprop("sim/hitches/aerotow/tow/dist", distance); + #print("distance=",distance); + + + ############################################# calc forces ################################################## + + # calc forces only for JSBSim-aircrafts + + # tow-end-forces must be reported in N to be consiststent to Yasim-aircrafts + # hitch-forces must be LBS to be consistent to the JSBSim "external_forces/.../magnitude" definition + + if ( getprop("sim/flight-model") == "jsb" ) { + #print("Force-Routine"); + + # check if the MP-aircraft properties have been updated. If not (maybe due to time-lag) bypass force calculation (use previous forces instead) + var mp_reported_dist = aimember.getNode("sim/hitches/aerotow/tow/dist").getValue(); + var mp_last_reported_dist = getprop("sim/hitches/aerotow/tow/mp_last_reported_dist"); + var mp_delta_reported_dist = mp_reported_dist - mp_last_reported_dist ; + setprop("sim/hitches/aerotow/tow/mp_last_reported_dist",mp_reported_dist); + var mp_delta_reported_dist2 = mp_delta_reported_dist * mp_delta_reported_dist ; # we need the absolute value + if ( (mp_delta_reported_dist2 > 0.0000001) or (mp_reported_dist < 0. )){ # we have the updated MP coordinates (no time lag) + # or the MP-aircraft is a non-interactive mp plane (mp_reported_dist = -1) + # => update forces else use the old forces! + + var breakforce_N = getprop("sim/hitches/aerotow/tow/brake-force"); # could be different in both aircrafts + + var isSlave = getprop("sim/hitches/aerotow/is-slave"); + if ( !isSlave ){ # if we are master, we have to calculate the forces + #print("master: calc forces"); + var elastic_constant = getprop("sim/hitches/aerotow/tow/elastic-constant"); + var towlength_m = getprop("sim/hitches/aerotow/tow/length"); + + var delta_towlength_m = distance - towlength_m; + #print("towlength_m= ", towlength_m , " elastic_constant= ", elastic_constant," delta_towlength_m= ", delta_towlength_m); + + if ( delta_towlength_m < 0. ) { + var forcetow_N = 0.; + } + else{ + var forcetow_N = elastic_constant * delta_towlength_m / towlength_m; + } + } # end !isSlave + else { # we are slave and get the forces from master + #print("slave: get forces"," aimember=",aimember.getName()); + # get forces + var forcetowX_N = aimember.getNode("sim/hitches/aerotow/tow/end-force-x").getValue() * 1; + var forcetowY_N = aimember.getNode("sim/hitches/aerotow/tow/end-force-y").getValue() * 1; + var forcetowZ_N = aimember.getNode("sim/hitches/aerotow/tow/end-force-z").getValue() * 1; + var forcetow_N = math.sqrt( forcetowX_N * forcetowX_N + forcetowY_N * forcetowY_N + forcetowZ_N * forcetowZ_N ); + } # end isSlave + + var forcetow_LBS = forcetow_N * 0.224809; # N -> LBF + #print(" forcetow_N ", forcetow_N , " distance ", distance," ", breakforce_N); + + if ( forcetow_N < breakforce_N ) { + + var distancepr = (myHitch_pos.distance_to(aiHitch_pos)); + + # correct a failure, if the projected length is larger than direct length + if (distancepr > distance) { distancepr = distance;} + + var alpha = math.acos( (distancepr / distance) ); + if ( aiHitch_pos.alt() > myHitch_pos.alt()) alpha = - alpha; + + var beta = ( aiHitchheadto - my_head_deg ) * 0.01745; + var gamma = my_pitch_deg * 0.01745; + var delta = my_roll_deg * 0.01745; + + var sina = math.sin(alpha); + var cosa = math.cos(alpha); + var sinb = math.sin(beta); + var cosb = math.cos(beta); + var sing = math.sin(gamma); + var cosg = math.cos(gamma); + var sind = math.sin(delta); + var cosd = math.cos(delta); + + #var forcetow = forcetow_N; # we deliver N to JSBSim + var forcetow = forcetow_LBS; # we deliver LBS to JSBSim + + # global forces: alpha beta + var fglobalx = forcetow * cosa * cosb; + var fglobaly = forcetow * cosa * sinb; + var fglobalz = forcetow * sina; + + # local forces by pitch: gamma + var flpitchx = fglobalx * cosg - fglobalz * sing; + var flpitchy = fglobaly; + var flpitchz = fglobalx * sing + fglobalz * cosg; + + # local forces by roll: delta + var flrollx = flpitchx; + var flrolly = flpitchy * cosd + flpitchz * sind; + var flrollz = - flpitchy * sind + flpitchz * cosd; + + # asigning to LOCAL coord of plane + var forcex = flrollx; + var forcey = flrolly; + var forcez = flrollz; + #print("fx=",forcex," fy=",forcey," fz=",forcez); + + # JSBSim-body-frame: x-> nose / y -> right wing / z -> down + # apply forces to hook (forces are in LBS or N see above) + var hitchname = getprop("sim/hitches/aerotow/basename_force_jsbsim"); + setprop("fdm/jsbsim/external_reactions/" ~ hitchname ~ "_x/magnitude", forcex); + setprop("fdm/jsbsim/external_reactions/" ~ hitchname ~ "_y/magnitude", forcey); + setprop("fdm/jsbsim/external_reactions/" ~ hitchname ~ "_z/magnitude", forcez); + + } # end force < break force + else { # rope is broken + props.globals.getNode("sim/hitches/aerotow/broken").setBoolValue(1); + #setprop("sim/messages/atc", sprintf("Oh no, the tow is broken")); + releaseHitch("aerotow"); # open=1 / forces=0 / remove towrope + } + + ############################################# report forces ############################################## + + # if we are connected to an MP aircraft and master + var nodeIsMpAircraft = getprop("sim/hitches/aerotow/tow/connected-to-mp-node"); + if ( nodeIsMpAircraft and !isSlave ){ + #print("report Forces"); + + # transform my hitch coordinates to cartesian earth coordinates + var myHitchCartEarth = geodtocart(myHitch_pos.lat(),myHitch_pos.lon(),myHitch_pos.alt() ); + var myHitchXearth_m = myHitchCartEarth[0]; + var myHitchYearth_m = myHitchCartEarth[1]; + var myHitchZearth_m = myHitchCartEarth[2]; + + # transform MP hitch coordinates to cartesian earth coordinates + var aiHitchCartEarth = geodtocart(aiHitch_pos.lat(),aiHitch_pos.lon(),aiHitch_pos.alt() ); + var aiHitchXearth_m = aiHitchCartEarth[0]; + var aiHitchYearth_m = aiHitchCartEarth[1]; + var aiHitchZearth_m = aiHitchCartEarth[2]; + + # calculate normal vector in tow direction in cartesian earth coordinates + var dx = aiHitchXearth_m - myHitchXearth_m; + var dy = aiHitchYearth_m - myHitchYearth_m; + var dz = aiHitchZearth_m - myHitchZearth_m; + var dl = math.sqrt( dx * dx + dy * dy + dz * dz ); + + var forcetowX_N = forcetow_N * dx / dl; + var forcetowY_N = forcetow_N * dy / dl; + var forcetowZ_N = forcetow_N * dz / dl; + + setprop("sim/hitches/aerotow/tow/dist", distance); + setprop("sim/hitches/aerotow/tow/end-force-x", -forcetowX_N); # force acts in + setprop("sim/hitches/aerotow/tow/end-force-y", -forcetowY_N); # opposite direction + setprop("sim/hitches/aerotow/tow/end-force-z", -forcetowZ_N); # at tow end + + } # end report forces + + } # end: timelag + else{ + #print("forces NOT updated!"); + } + } # end forces/JSBSim + + + } # end: aiNodeID + else{ + if ( !aimember.getNode("valid").getValue() ) { # MP-aircraft is accessable (node is valid) + #print("MP-aircraft isn't valid!"); + props.globals.getNode("sim/hitches/aerotow/open").setBoolValue(1); # open my hitch + setprop("sim/messages/atc", sprintf("MP-aircraft disappeared!" )); + } + } + } # end: check id != nil + } # end: loop over aiobjects + + +} # end function aerotow + + + +# ###################################################################################################################### +# winch function +# ###################################################################################################################### + +var winch = func (open){ + + var FT2M = 0.30480; + var M2FT = 1. / FT2M; + var RAD2DEG = 57.29578; + var DEG2RAD = 1. / RAD2DEG; + + if (!open ) { + + ########################################### my hitch position ############################################ + + myPosition = geo.aircraft_position(); + var my_head_deg = getprop("orientation/heading-deg"); + var my_roll_deg = getprop("orientation/roll-deg"); + var my_pitch_deg = getprop("orientation/pitch-deg"); + + # hitch coordinates in YASim-system (x-> nose / y -> left wing / z -> up) + var x = getprop("sim/hitches/winch/local-pos-x"); + var y = getprop("sim/hitches/winch/local-pos-y"); + var z = getprop("sim/hitches/winch/local-pos-z"); + + var alpha_deg = my_roll_deg * (1.); # roll clockwise (looking in x-direction) := + + var beta_deg = my_pitch_deg * (-1.); # pitch clockwise (looking in y-direction) := - + + # transform hook coordinates + 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.); + + var install_distance_m = Xn[0]; # in front of ref-point of glider + var install_side_m = Xn[1]; + var install_alt_m = Xn[2]; + + var myHitch_pos = myPosition.apply_course_distance( my_head_deg , install_distance_m ); + var myHitch_pos = myPosition.apply_course_distance( my_head_deg - 90. , install_side_m ); + myHitch_pos.set_alt(myPosition.alt() + install_alt_m); + + ########################################### winch hitch position ############################################ + + # get coordinates + var winch_global_pos_x = getprop("sim/hitches/winch/winch/global-pos-x"); + var winch_global_pos_y = getprop("sim/hitches/winch/winch/global-pos-y"); + var winch_global_pos_z = getprop("sim/hitches/winch/winch/global-pos-z"); + + var winch_geod = carttogeod(winch_global_pos_x,winch_global_pos_y,winch_global_pos_z); + + var ai_lat = winch_geod[0]; + var ai_lon = winch_geod[1]; + #var ai_alt = winch_geod[2] * FT2M; + var ai_alt = winch_geod[2]; + #print("ai_lat,lon,alt",ai_lat,ai_lon,ai_alt); + + var aiHitch_pos = geo.Coord.set_latlon( ai_lat, ai_lon, ai_alt ); + + + ########################################### distance between hitches ##################################### + + var distance = (myHitch_pos.direct_distance_to(aiHitch_pos)); # distance to winch in meter + var aiHitchheadto = (myHitch_pos.course_to(aiHitch_pos)); + var height = myHitch_pos.alt() - aiHitch_pos.alt(); + + var aiHitchpitchto = -math.asin((myHitch_pos.alt()-aiHitch_pos.alt())/distance) / 0.01745; + #print(" pitch: ", aiHitchpitchto); + + # update position of rope + setprop("ai/models/winchrope/position/latitude-deg", myHitch_pos.lat()); + setprop("ai/models/winchrope/position/longitude-deg", myHitch_pos.lon()); + setprop("ai/models/winchrope/position/altitude-ft", myHitch_pos.alt() * M2FT); + #print("ai_lat,lon,alt",myHitch_pos.lat()," ",myHitch_pos.lon()," ",myHitch_pos.alt() ); + + # update pitch and heading of rope + setprop("ai/models/winchrope/orientation/true-heading-deg", aiHitchheadto); + setprop("ai/models/winchrope/orientation/pitch-deg", aiHitchpitchto); + + # update length of rope + setprop("sim/hitches/winch/tow/dist", distance); + #print("distance=",distance); + + + ############################################# calc forces ################################################## + + # calc forces only for JSBSim-aircrafts + + # tow-end-forces must be reported in N to be consiststent to Yasim-aircrafts + # hitch-forces must be LBS to be consistent to the JSBSim "external_forces/.../magnitude" definition + + if ( getprop("sim/flight-model") == "jsb" ) { + + var spool_max = getprop("sim/hitches/winch/winch/max-spool-speed-m-s"); + var unspool_max = getprop("sim/hitches/winch/winch/max-unspool-speed-m-s"); + var max_force_N = getprop("sim/hitches/winch/winch/max-force-N"); + var max_power_W = getprop("sim/hitches/winch/winch/max-power-kW") * 1000.; + var breakforce_N = getprop("sim/hitches/winch/tow/break-force-N"); + var elastic_constant = getprop("sim/hitches/winch/tow/elastic-constant"); + var towlength_m = getprop("sim/hitches/winch/tow/length"); + var max_tow_length_m = getprop("sim/hitches/winch/winch/max-tow-length-m"); + var spoolspeed = getprop("sim/hitches/winch/winch/actual-spool-speed-m-s"); + var spool_acceleration = getprop("sim/hitches/winch/winch/spool-acceleration-m-s-s"); + var delta_t = getprop("sim/time/delta-sec"); + + var towlength_new_m = towlength_m - spoolspeed * delta_t; + var delta_towlength_m = distance - towlength_new_m; + #print("towlength_m= ", towlength_m , " elastic_constant= ", elastic_constant," delta_towlength_m= ", delta_towlength_m); + + if ( getprop("sim/hitches/winch/winch/clutched") ) { + var delta_spoolspeed = spool_acceleration * delta_t; + spoolspeed = spoolspeed + delta_spoolspeed ; + if ( spoolspeed > spool_max ) spoolspeed = spool_max; + } + else { # un-clutched + # --- experimental --- # + + # we assume that the the winch-operator avoids tow sagging ( => rigid rope; negativ forces allowed) + var forcetow_N = elastic_constant * delta_towlength_m / towlength_new_m; + + # drag of tow-rope ( magic! ) + var magic_constant = getprop("sim/hitches/winch/winch/magic-constant"); + tow_drag_N = spoolspeed * spoolspeed * math.sqrt( math.sqrt( height * height ) * max_tow_length_m ) / magic_constant ; + + # mass = tow-mass only (drum-mass ignored) + var mass_kg = max_tow_length_m * getprop("sim/hitches/winch/tow/weight-per-m-kg-m"); + + var acceleration = ( forcetow_N - tow_drag_N ) / mass_kg; + var delta_spoolspeed = acceleration * delta_t; + spoolspeed = spoolspeed - delta_spoolspeed; + if ( spoolspeed < - unspool_max ) spoolspeed = - unspool_max; + #print("spoolspeed= ",spoolspeed," delta_spoolspeed= ",delta_spoolspeed," delta_towlength= ", delta_towlength_m); + #print("forcetow_N= ",forcetow_N," tow_drag_N= ",tow_drag_N," acceleration= ", acceleration); + } + + if ( delta_towlength_m < 0. ) { + var forcetow_N = 0.; + } + else{ + var forcetow_N = elastic_constant * delta_towlength_m / towlength_new_m; + } + + if ( forcetow_N > max_force_N ) { + forcetow_N = max_force_N; + var towlength_new_m = distance / ( forcetow_N / elastic_constant + 1. ); + spoolspeed = (towlength_m - towlength_new_m ) / delta_t; + } + + var power = forcetow_N * spoolspeed; + if ( power > max_power_W) { + power = max_power_W; + spoolspeed = power / forcetow_N; + towlength_new_m = towlength_m - spoolspeed * delta_t; + } + #print("power=",power," spoolspeed=",spoolspeed," force=",forcetow_N); + + setprop("sim/hitches/winch/tow/length",towlength_new_m); + setprop("sim/hitches/winch/winch/actual-spool-speed-m-s",spoolspeed); + setprop("sim/hitches/winch/winch/actual-force-N",forcetow_N); + + # force due to tow-weight (acts in tow direction at the heigher hitch) + var force_due_to_weight_N = getprop("sim/hitches/winch/tow/weight-per-m-kg-m") * 9.81 * height; + if (height < 0. ) force_due_to_weight_N = 0.; + + forcetow_N = forcetow_N + force_due_to_weight_N; + var forcetow_LBS = forcetow_N * 0.224809; # N -> LBF + #print(" forcetow_N ", forcetow_N , " distance ", distance," ", breakforce_N); + #print(" forcetow_N=", forcetow_N , " force_due_to_weight_N=", force_due_to_weight_N," height=",height); + + if ( forcetow_N < breakforce_N ) { + + var distancepr = (myHitch_pos.distance_to(aiHitch_pos)); + + # correct a failure, if the projected length is larger than direct length + if (distancepr > distance) { distancepr = distance;} + + var alpha = math.acos( (distancepr / distance) ); + if ( aiHitch_pos.alt() > myHitch_pos.alt()) alpha = - alpha; + var beta = ( aiHitchheadto - my_head_deg ) * DEG2RAD; + var gamma = my_pitch_deg * DEG2RAD; + var delta = my_roll_deg * DEG2RAD; + + var sina = math.sin(alpha); + var cosa = math.cos(alpha); + var sinb = math.sin(beta); + var cosb = math.cos(beta); + var sing = math.sin(gamma); + var cosg = math.cos(gamma); + var sind = math.sin(delta); + var cosd = math.cos(delta); + + #var forcetow = forcetow_N; # we deliver N to JSBSim + var forcetow = forcetow_LBS; # we deliver LBS to JSBSim + + # global forces: alpha beta + var fglobalx = forcetow * cosa * cosb; + var fglobaly = forcetow * cosa * sinb; + var fglobalz = forcetow * sina; + + # local forces by pitch: gamma + var flpitchx = fglobalx * cosg - fglobalz * sing; + var flpitchy = fglobaly; + var flpitchz = fglobalx * sing + fglobalz * cosg; + + # local forces by roll: delta + var flrollx = flpitchx; + var flrolly = flpitchy * cosd + flpitchz * sind; + var flrollz = - flpitchy * sind + flpitchz * cosd; + + # asigning to LOCAL coord of plane + var forcex = flrollx; + var forcey = flrolly; + var forcez = flrollz; + #print("fx=",forcex," fy=",forcey," fz=",forcez); + + # JSBSim-body-frame: x-> nose / y -> right wing / z -> down + # apply forces to hook (forces are in LBS or N see above) + var hitchname = getprop("sim/hitches/winch/basename_force_jsbsim"); + setprop("fdm/jsbsim/external_reactions/" ~ hitchname ~ "_x/magnitude", forcex); + setprop("fdm/jsbsim/external_reactions/" ~ hitchname ~ "_y/magnitude", forcey); + setprop("fdm/jsbsim/external_reactions/" ~ hitchname ~ "_z/magnitude", forcez); + + # check, if auto-release condition is reached + var rope_angle_deg = math.atan2(forcez , forcex ) * RAD2DEG; + #print("rope_angle_deg=",rope_angle_deg); + if (rope_angle_deg > getprop("sim/hitches/winch/automatic-release-angle-deg") ) releaseWinch(); + + } # end force < break force + else { # rope is broken + props.globals.getNode("sim/hitches/winch/broken").setBoolValue(1); + releaseWinch(); + } + + if ( towlength_new_m > max_tow_length_m ) { + setprop("sim/messages/atc", sprintf("tow length exceeded!")); + releaseWinch(); + } + + } # end forces/JSBSim + + } # end hitch is closed (open == 0) + +} # end function winch + + +# ###################################################################################################################### +# create towrope +# ###################################################################################################################### + +var createTowrope = func (device){ + + # create the towrope in the model property tree + #print("createTowrope for ",device); + + if ( getprop("sim/hitches/" ~ device ~ "/rope/exist") == 0 ) { # does the towrope exist? + + # get the next free model id + var freeModelid = getFreeModelID(); + + props.globals.getNode("sim/hitches/" ~ device ~ "/rope/model_id").setIntValue(freeModelid); + props.globals.getNode("sim/hitches/" ~ device ~ "/rope/exist").setBoolValue(1); + + var towrope_ai = props.globals.getNode("ai/models/" ~ device ~ "rope", 1); + var towrope_mod = props.globals.getNode("models", 1); + + towrope_ai.getNode("id", 1).setIntValue(4711); + towrope_ai.getNode("callsign", 1).setValue("towrope"); + towrope_ai.getNode("valid", 1).setBoolValue(1); + towrope_ai.getNode("position/latitude-deg", 1).setValue(0.); + towrope_ai.getNode("position/longitude-deg", 1).setValue(0.); + towrope_ai.getNode("position/altitude-ft", 1).setValue(0.); + towrope_ai.getNode("orientation/true-heading-deg", 1).setValue(0.); + towrope_ai.getNode("orientation/pitch-deg", 1).setValue(0.); + towrope_ai.getNode("orientation/roll-deg", 1).setValue(0.); + + towrope_mod.model = towrope_mod.getChild("model", freeModelid, 1); + towrope_mod.model.getNode("path", 1).setValue(getprop("sim/hitches/" ~ device ~ "/rope/path_to_model") ); + towrope_mod.model.getNode("longitude-deg-prop", 1).setValue("ai/models/" ~ device ~ "rope/position/longitude-deg"); + towrope_mod.model.getNode("latitude-deg-prop", 1).setValue("ai/models/" ~ device ~ "rope/position/latitude-deg"); + towrope_mod.model.getNode("elevation-ft-prop", 1).setValue("ai/models/" ~ device ~ "rope/position/altitude-ft"); + towrope_mod.model.getNode("heading-deg-prop", 1).setValue("ai/models/" ~ device ~ "rope/orientation/true-heading-deg"); + towrope_mod.model.getNode("roll-deg-prop", 1).setValue("ai/models/" ~ device ~ "rope/orientation/roll-deg"); + towrope_mod.model.getNode("pitch-deg-prop", 1).setValue("ai/models/" ~ device ~ "rope/orientation/pitch-deg"); + towrope_mod.model.getNode("load", 1).remove(); + } # end towrope exist +} + + +# ###################################################################################################################### +# get the next free id of "models/model" members +# ###################################################################################################################### + +var getFreeModelID = func { + #print("getFreeModelID"); + var modelid = 0; # next unused id + modelobjects = props.globals.getNode("models", 1).getChildren(); + foreach ( var member; modelobjects ) { + if ( (var c = member.getIndex()) != nil) { + modelid = c + 1; + } + } + #print("modelid=",modelid); + return(modelid); +} + + +# ###################################################################################################################### +# close hitch +# ###################################################################################################################### + +var closeHitch = func { + + #print("closeHitch"); + + setprop("sim/hitches/aerotow/open", "false"); + setprop("sim/hitches/aerotow/mp_oldOpen", "true"); + +} # End function closeHitch + + +# ###################################################################################################################### +# release hitch +# ###################################################################################################################### + +var releaseHitch = func (device){ + + #print("releaseHitch"); + + if ( getprop("sim/flight-model") == "yasim" ) return; # bypass this routine for Yasim-aircrafts + + setprop("sim/hitches/" ~ device ~ "/open", "true"); + + var hitchname = getprop("sim/hitches/" ~ device ~ "/basename_force_jsbsim"); + setprop("fdm/jsbsim/external_reactions/" ~ hitchname ~ "_x/magnitude", 0.); + setprop("fdm/jsbsim/external_reactions/" ~ hitchname ~ "_y/magnitude", 0.); + setprop("fdm/jsbsim/external_reactions/" ~ hitchname ~ "_z/magnitude", 0.); + + if ( device == "aerotow" ) { + setprop("sim/hitches/aerotow/tow/end-force-x", 0.); # MP tow-end forces + setprop("sim/hitches/aerotow/tow/end-force-y", 0.); # + setprop("sim/hitches/aerotow/tow/end-force-z", 0.); # + } + +} # End function releaseHitch + + +# ###################################################################################################################### +# remove/delete towrope +# ###################################################################################################################### + +var removeTowrope = func (device){ + + # remove the towrope from the property tree ai/models + # remove the towrope from the property tree models/ + + if ( getprop("sim/hitches/" ~ device ~ "/rope/exist") == 1 ) { # does the towrope exist? + + # remove 3d model from scenery + # identification is /models/model[x] with x=id_model + var id_model = getprop("sim/hitches/" ~ device ~ "/rope/model_id"); + var modelsNode = "models/model[" ~ id_model ~ "]"; + props.globals.getNode(modelsNode).remove(); + props.globals.getNode("ai/models/" ~ device ~ "rope").remove(); + #print("towrope removed"); + setprop("sim/hitches/" ~ device ~ "/rope/exist", 0); + } + +} + + +# ###################################################################################################################### +# pull in towrope after hitch has been opened +# ###################################################################################################################### + +var pull_in_rope = func { + + var deg2rad = math.pi / 180.; + var FT2M = 0.30480; + + if ( getprop("sim/hitches/winch/open") ) { + + # get length of rope + #var distance = getprop("sim/hitches/winch/tow/dist"); + + var towlength_m = getprop("sim/hitches/winch/tow/length"); + var spoolspeed = getprop("sim/hitches/winch/winch/max-spool-speed-m-s"); + var delta_t = getprop("sim/time/delta-sec"); + + var delta_length_m = spoolspeed * delta_t; + var towlength_new_m = towlength_m - delta_length_m; + var towlength_min_m = getprop("sim/hitches/winch/winch/min-tow-length-m"); + + if ( towlength_new_m > towlength_min_m ) { + #print("actual towlength= ",towlength_new_m); + + # get position of rope end (former myHitch_pos) + var tow_lat = getprop("ai/models/winchrope/position/latitude-deg"); + var tow_lon = getprop("ai/models/winchrope/position/longitude-deg"); + var tow_alt_m = getprop("ai/models/winchrope/position/altitude-ft") * FT2M; + # get pitch and heading of rope + var tow_heading_deg = getprop("ai/models/winchrope/orientation/true-heading-deg"); + var tow_pitch_rad = getprop("ai/models/winchrope/orientation/pitch-deg") * deg2rad; + + var aiTow_pos = geo.Coord.set_latlon( tow_lat, tow_lon, tow_alt_m ); + + var delta_distance_m = delta_length_m * math.cos(tow_pitch_rad); + var delta_alt_m = delta_length_m * math.sin(tow_pitch_rad); + # vertical sink rate not yet taken into account! + aiTow_pos = aiTow_pos.apply_course_distance( tow_heading_deg , delta_distance_m ); + aiTow_pos.set_alt(tow_alt_m + delta_alt_m); + #print("aiTow_pos.alt()= ",aiTow_pos.alt()," ",tow_alt_m + delta_alt_m); + + # update position of rope + setprop("ai/models/winchrope/position/latitude-deg", aiTow_pos.lat()); + setprop("ai/models/winchrope/position/longitude-deg", aiTow_pos.lon()); + setprop("ai/models/winchrope/position/altitude-ft", aiTow_pos.alt() * M2FT); + + # update length of rope + setprop("sim/hitches/winch/tow/length",towlength_new_m); + + settimer( pull_in_rope , 0 ); + } # end towlength > min + else { + #print("pull in finished!"); + setprop("sim/hitches/winch/winch/actual-spool-speed-m-s", 0. ); + removeTowrope("winch"); # remove towrope model + } + + } # end if open + +} + + +# ###################################################################################################################### +# set some AI-object default values +# ###################################################################################################################### + +var setAIObjectDefaults = func (){ + + # set some default variables, needed to identify, if the found object is an AI-object, a "non-interactiv MP-object or + # an interactive MP-object + + var aiNodeID = getprop("sim/hitches/aerotow/tow/connected-to-ai-or-mp-id"); # id of former found ai/mp aircraft + + aiobjects = props.globals.getNode("ai/models").getChildren(); + foreach (var aimember; aiobjects) { + if ( (var c = aimember.getNode("id") ) != nil ) { + var testprop = c.getValue(); + if ( testprop == aiNodeID) { + # Set some dummy values. In case of an "interactive"-MP plane + # the correct values will be transmitted in the following loop. + # Create this variables if not present. + aimember.getNode("sim/hitches/aerotow/local-pos-x",1).setValue(-5.); + aimember.getNode("sim/hitches/aerotow/local-pos-y",1).setValue(0.); + aimember.getNode("sim/hitches/aerotow/local-pos-z",1).setValue(0.); + aimember.getNode("sim/hitches/aerotow/tow/dist",1).setValue(-1.); + } + } + } + +} + + +# ###################################################################################################################### +# place winch model +# ###################################################################################################################### + +var setWinchPositionAuto = func { + + # remove already existing winch model + if ( getprop("/sim/hitches/winch/winch/winch-model-index") != nil ) { + var id_model = getprop("/sim/hitches/winch/winch/winch-model-index"); + var modelsNode = "models/model[" ~ id_model ~ "]"; + props.globals.getNode(modelsNode).remove(); + #print("winch model removed"); + } + + var initial_length_m = getprop("sim/hitches/winch/winch/initial-tow-length-m"); + var ac_pos = geo.aircraft_position(); # get position of aircraft + var ac_hd = getprop("orientation/heading-deg"); # get heading of aircraft + + # setup winch + # get initial runway position + var ipos_lat_deg = getprop("sim/presets/latitude-deg"); + var ipos_lon_deg = getprop("sim/presets/longitude-deg"); + var ipos_hd_deg = getprop("sim/presets/heading-deg"); + var ipos_alt_m = geo.elevation(ipos_lat_deg,ipos_lon_deg); + var ipos_geo = geo.Coord.new().set_latlon(ipos_lat_deg, ipos_lon_deg, ipos_alt_m); + # offset to initial position + var deviation = (ac_pos.distance_to(ipos_geo)); + # if deviation is too much, locate winch in front of glider, otherwise locate winch to end of runway + if ( deviation > 200) { + var w = ac_pos.apply_course_distance( ac_hd , initial_length_m -1. ); + } + else { + var w = ipos_geo.apply_course_distance( ipos_hd_deg , initial_length_m - 1. ); + } + var wpalt = geo.elevation(w.lat(), w.lon()); + w.set_alt(wpalt); + + var winchModel = geo.put_model("Models/Airport/supacat_winch.xml", w.lat(), w.lon(), (w.alt()+0.81), (w.course_to(ac_pos) )); + + setprop("/sim/hitches/winch/winch/global-pos-x", w.x()); + setprop("/sim/hitches/winch/winch/global-pos-y", w.y()); + setprop("/sim/hitches/winch/winch/global-pos-z", w.z()); + + setprop("sim/hitches/winch/tow/dist",initial_length_m - 1.); + setprop("sim/hitches/winch/tow/length",initial_length_m); + + #print("name=",winchModel.getName()," Index=",winchModel.getIndex()," Type=",winchModel.getType() ); + #print("val=",winchModel.getValue()," children=",winchModel.getChildren()," size=",size(winchModel) ); + setprop("/sim/hitches/winch/winch/winch-model-index",winchModel.getIndex() ); + setprop("sim/messages/pilot", sprintf("Connected to winch!")); + + props.globals.getNode("sim/hitches/winch/open").setBoolValue(0); + +} # End function setWinchPositionAuto + + +# ###################################################################################################################### +# clutch / un-clutch winch +# ###################################################################################################################### + +var runWinch = func { + + if ( !getprop("sim/hitches/winch/winch/clutched") ) { + setprop("sim/hitches/winch/winch/clutched","true"); + setprop("sim/messages/pilot", sprintf("Winch clutched!")); + } + else { + setprop("sim/hitches/winch/winch/clutched","false"); + setprop("sim/messages/pilot", sprintf("Winch un-clutched!")); + } + +} # End function runWinch + + +# ###################################################################################################################### +# release winch +# ###################################################################################################################### + +var releaseWinch = func { + + setprop("sim/hitches/winch/open","true"); + +} # End function releaseWinch + + +# ###################################################################################################################### +# point transformation +# ###################################################################################################################### + +var PointRotate3D = func (x,y,z,xr,yr,zr,alpha_deg,beta_deg,gamma_deg){ + + # --------------------------------------------------------------------------------- + # rotates point (x,y,z) about all 3 cartesian axis + # center of rotation (xr,yr,zr) + # angle of rotation about x-axis = alpha + # angle of rotation about y-axis = beta + # angle of rotation about z-axis = gamma + # delivers new point coordinates (x_new,y_new,z_new) + # --------------------------------------------------------------------------------- + # + # + # Definitions: + # ---------------- + # + # x y z + # alpha beta gamma + # + # + # z + # | y + # | / + # |/ + # ----->x + # + #---------------------------------------------------------------------------------- + + # Transformation in rotation-system X_rel = X-Xr = (x-xr, y-yr, z-zr) + var x_rel = x-xr; + var y_rel = y-yr; + var z_rel = z-zr; + + # Trigonometry + var deg2rad = math.pi / 180.; + + var alpha_rad = deg2rad * alpha_deg; + var beta_rad = deg2rad * beta_deg; + var gamma_rad = deg2rad * gamma_deg; + + var sin_alpha = math.sin(alpha_rad); + var cos_alpha = math.cos(alpha_rad); + + var sin_beta = math.sin(beta_rad); + var cos_beta = math.cos(beta_rad); + + var sin_gamma = math.sin(gamma_rad); + var cos_gamma = math.cos(gamma_rad); + + # Matrices + # + # Rotate about x-axis Rx(alpha) + # + # Rx11 Rx12 Rx13 1 0 0 + # Rx(alpha)= Rx21 Rx22 Rx23 = 0 cos(alpha) -sin(alpha) + # Rx31 Rx32 Rx33 0 sin(alpha) cos(alpha) + # + var Rx11 = 1.; + var Rx12 = 0.; + var Rx13 = 0.; + var Rx21 = 0.; + var Rx22 = cos_alpha; + var Rx23 = - sin_alpha; + var Rx31 = 0.; + var Rx32 = sin_alpha; + var Rx33 = cos_alpha; + # + # Rotate about y-axis Ry(beta) + # + # Ry11 Ry12 Ry13 cos(beta) 0 sin(beta) + # Ry(beta)= Ry21 Ry22 Ry23 = 0 1 0 + # Ry31 Ry32 Ry33 -sin(beta) 0 cos(beta) + # + var Ry11 = cos_beta; + var Ry12 = 0.; + var Ry13 = sin_beta; + var Ry21 = 0.; + var Ry22 = 1.; + var Ry23 = 0.; + var Ry31 = - sin_beta; + var Ry32 = 0.; + var Ry33 = cos_beta; + # + # Rotate about z-axis Rz(gamma) + # + # Rz11 Rz12 Rz13 cos(gamma) -sin(gamma) 0 + # Rz(gamma)= Rz21 Rz22 Rz23 = sin(gamma) cos(gamma) 0 + # Rz31 Rz32 Rz33 0 0 1 + # + var Rz11 = cos_gamma; + var Rz12 = - sin_gamma; + var Rz13 = 0.; + var Rz21 = sin_gamma; + var Rz22 = cos_gamma; + var Rz23 = 0.; + var Rz31 = 0.; + var Rz32 = 0.; + var Rz33 = 1.; + # + # First rotation about x-axis + # X_x = Rx*X_rel + var x_x = Rx11 * x_rel + Rx12 * y_rel + Rx13 * z_rel; + var y_x = Rx21 * x_rel + Rx22 * y_rel + Rx23 * z_rel; + var z_x = Rx31 * x_rel + Rx32 * y_rel + Rx33 * z_rel; + # + # subsequent rotation about y-axis + # X_xy = Ry*X_x + var x_xy = Ry11 * x_x + Ry12 * y_x + Ry13 * z_x; + var y_xy = Ry21 * x_x + Ry22 * y_x + Ry23 * z_x; + var z_xy = Ry31 * x_x + Ry32 * y_x + Ry33 * z_x; + # + # subsequent rotation about z-axis: + # X_xyz = Rz*X_xy + var x_xyz = Rz11 * x_xy + Rz12 * y_xy + Rz13 * z_xy; + var y_xyz = Rz21 * x_xy + Rz22 * y_xy + Rz23 * z_xy; + var z_xyz = Rz31 * x_xy + Rz32 * y_xy + Rz33 * z_xy; + + # Back transformation X_rel = X-Xr = (x-xr, y-yr, z-zr) + var xn = xr + x_xyz; + var yn = yr + y_xyz; + var zn = zr + z_xyz; + + var Xn = [xn,yn,zn]; + + return Xn; + +} + +################################################################################################################################## + + +# todo: +# ------ +# +# - animate rope slack +# - pull in towrope: take sink rate of rope into account +# - dynamic ID for ai-rope-model +# +# Please contact D-NXKT at yahoo.de for bug-reports, suggestions, ... +#