src/FDM/YASim/Hitch.cpp

   1 #include "Math.hpp"
   2 #include "BodyEnvironment.hpp"
   3 #include "RigidBody.hpp"
   4 #include <string.h>
   5 #include <sstream>
   6
   7
   8
   9 #include "Hitch.hpp"
  10
  11 using std::vector;
  12
  13 namespace yasim {
  14 Hitch::Hitch(const char *name)
  15 {
  16     if (fgGetNode("/sim/hitches", true))
  17         _node = fgGetNode("/sim/hitches", true)->getNode(name, true);
  18     else _node = 0;
  19     int i;
  20     for(i=0; i<3; i++)
  21         _pos[i] = _force[i] = _winchPos[i] = _mp_lpos[i]=_towEndForce[i]=_mp_force[i]=0;
  22     for(i=0; i<2; i++)
  23         _global_ground[i] = 0;
  24     _global_ground[2] = 1;
  25     _global_ground[3] = -1e5;
  26     _forceMagnitude=0;
  27     _open=true;
  28     _oldOpen=_open;
  29     _towLength=60;
  30     _towElasticConstant=1e5;
  31     _towBrakeForce=100000;
  32     _towWeightPerM=1;
  33     _winchMaxSpeed=40;
  34     _winchRelSpeed=0;
  35     _winchInitialTowLength=1000;
  36     _winchPower=100000;
  37     _winchMaxForce=10000;
  38     _winchActualForce=0;
  39     _winchMaxTowLength=1000;
  40     _winchMinTowLength=0;
  41     _dist=0;
  42     _towEndIsConnectedToProperty=false;
  43     _towEndNode=0;
  44     _nodeIsMultiplayer=false;
  45     _nodeIsAiAircraft=false;
  46     _forceIsCalculatedByMaster=false;
  47     _nodeID=0;
  48     //_ai_MP_callsign=0;
  49     _height_above_ground=0;
  50     _winch_height_above_ground=0;
  51     _loPosFrac=0;
  52     _lowest_tow_height=0;
  53     _state=new State;
  54     _displayed_len_lower_dist_message=false;
  55     _last_wish=true;
  56     _isSlave=false;
  57     _mpAutoConnectPeriod=0;
  58     _timeToNextAutoConnectTry=0;
  59     _timeToNextReConnectTry=10;
  60     _speed_in_tow_direction=0;
  61     _mp_time_lag=1;
  62     _mp_last_reported_dist=0;
  63     _mp_last_reported_v=0;
  64     _mp_is_slave=false;
  65     _mp_open_last_state=false;
  66     _timeLagCorrectedDist=0;
  67
  68     if (_node)
  69     {
  70 #define TIE(x,v) _tiedProperties.Tie(_node->getNode(x, true),v)
  71         TIE("tow/length", SGRawValuePointer<float>(&_towLength));
  72         TIE("tow/elastic-constant",SGRawValuePointer<float>(&_towElasticConstant));
  73         TIE("tow/weight-per-m-kg-m",SGRawValuePointer<float>(&_towWeightPerM));
  74         TIE("tow/brake-force",SGRawValuePointer<float>(&_towBrakeForce));
  75         TIE("winch/max-speed-m-s",SGRawValuePointer<float>(&_winchMaxSpeed));
  76         TIE("winch/rel-speed",SGRawValuePointer<float>(&_winchRelSpeed));
  77         TIE("winch/initial-tow-length-m",SGRawValuePointer<float>(&_winchInitialTowLength));
  78         TIE("winch/min-tow-length-m",SGRawValuePointer<float>(&_winchMinTowLength));
  79         TIE("winch/max-tow-length-m",SGRawValuePointer<float>(&_winchMaxTowLength));
  80         TIE("winch/global-pos-x",SGRawValuePointer<double>(&_winchPos[0]));
  81         TIE("winch/global-pos-y",SGRawValuePointer<double>(&_winchPos[1]));
  82         TIE("winch/global-pos-z",SGRawValuePointer<double>(&_winchPos[2]));
  83         TIE("winch/max-power",SGRawValuePointer<float>(&_winchPower));
  84         TIE("winch/max-force",SGRawValuePointer<float>(&_winchMaxForce));
  85         TIE("winch/actual-force",SGRawValuePointer<float>(&_winchActualForce));
  86         TIE("tow/end-force-x",SGRawValuePointer<float>(&_reportTowEndForce[0]));
  87         TIE("tow/end-force-y",SGRawValuePointer<float>(&_reportTowEndForce[1]));
  88         TIE("tow/end-force-z",SGRawValuePointer<float>(&_reportTowEndForce[2]));
  89         TIE("force",SGRawValuePointer<float>(&_forceMagnitude));
  90         TIE("open",SGRawValuePointer<bool>(&_open));
  91         TIE("force-is-calculated-by-other",SGRawValuePointer<bool>(&_forceIsCalculatedByMaster));
  92         TIE("local-pos-x",SGRawValuePointer<float>(&_pos[0]));
  93         TIE("local-pos-y",SGRawValuePointer<float>(&_pos[1]));
  94         TIE("local-pos-z",SGRawValuePointer<float>(&_pos[2]));
  95         TIE("tow/dist",SGRawValuePointer<float>(&_dist));
  96         TIE("tow/dist-time-lag-corrected",SGRawValuePointer<float>(&_timeLagCorrectedDist));
  97         TIE("tow/connected-to-property-node",SGRawValuePointer<bool>(&_towEndIsConnectedToProperty));
  98         TIE("tow/connected-to-mp-node",SGRawValuePointer<bool>(&_nodeIsMultiplayer));
  99         TIE("tow/connected-to-ai-node",SGRawValuePointer<bool>(&_nodeIsAiAircraft));
 100         TIE("tow/connected-to-ai-or-mp-id",SGRawValuePointer<int>(&_nodeID));
 101         TIE("debug/hitch-height-above-ground",SGRawValuePointer<float>(&_height_above_ground));
 102         TIE("debug/tow-end-height-above-ground",SGRawValuePointer<float>(&_winch_height_above_ground));
 103         TIE("debug/tow-rel-lo-pos",SGRawValuePointer<float>(&_loPosFrac));
 104         TIE("debug/tow-lowest-pos-height",SGRawValuePointer<float>(&_lowest_tow_height));
 105         TIE("is-slave",SGRawValuePointer<bool>(&_isSlave));
 106         TIE("speed-in-tow-direction",SGRawValuePointer<float>(&_speed_in_tow_direction));
 107         TIE("mp-auto-connect-period",SGRawValuePointer<float>(&_mpAutoConnectPeriod));
 108         TIE("mp-time-lag",SGRawValuePointer<float>(&_mp_time_lag));
 109 #undef TIE
 110         _node->setStringValue("tow/node","");
 111         _node->setStringValue("tow/connected-to-ai-or-mp-callsign");
 112         _node->setBoolValue("broken",false);
 113     }
 114 }
 115
 116 Hitch::~Hitch()
 117 {
 118     _tiedProperties.Untie();
 119     delete _state;
 120 }
 121
 122 void Hitch::setPosition(float* position)
 123 {
 124     int i;
 125     for(i=0; i<3; i++) _pos[i] = position[i];
 126 }
 127
 128 void Hitch::setTowLength(float length)
 129 {
 130     _towLength = length;
 131 }
 132
 133 void Hitch::setOpen(bool isOpen)
 134 {
 135    //test if we already processed this before
 136     //without this test a binded property could
 137     //try to close the Hitch every run
 138     //it will close, if we are near the end
 139     //e.g. if we are flying over the parked
 140     //tow-aircraft....
 141     if (isOpen==_last_wish)
 142         return;
 143     _last_wish=isOpen;
 144     _open=isOpen;
 145 }
 146
 147 void Hitch::setTowElasticConstant(float sc)
 148 {
 149     _towElasticConstant=sc;
 150 }
 151
 152 void Hitch::setTowBreakForce(float bf)
 153 {
 154     _towBrakeForce=bf;
 155 }
 156
 157 void Hitch::setWinchMaxForce(float f)
 158 {
 159     _winchMaxForce=f;
 160 }
 161
 162 void Hitch::setTowWeightPerM(float rw)
 163 {
 164     _towWeightPerM=rw;
 165 }
 166
 167 void Hitch::setWinchMaxSpeed(float mws)
 168 {
 169     _winchMaxSpeed=mws;
 170 }
 171
 172 void Hitch::setWinchRelSpeed(float rws)
 173 {
 174     _winchRelSpeed=rws;
 175 }
 176
 177 void Hitch::setWinchPosition(double *winchPosition)//in global coordinates!
 178 {
 179     for (int i=0; i<3;i++)
 180         _winchPos[i]=winchPosition[i];
 181 }
 182
 183 void Hitch::setMpAutoConnectPeriod(float dt)
 184 {
 185     _mpAutoConnectPeriod=dt;
 186 }
 187
 188 void Hitch::setForceIsCalculatedByOther(bool b)
 189 {
 190     _forceIsCalculatedByMaster=b;
 191 }
 192
 193 std::string Hitch::getConnectedPropertyNode() const
 194 {
 195     if (_towEndNode)
 196         return _towEndNode->getDisplayName();
 197     else
 198         return std::string("");
 199 }
 200
 201 void Hitch::setConnectedPropertyNode(const char *nodename)
 202 {
 203     _towEndNode=fgGetNode(nodename,false);
 204 }
 205
 206 void Hitch::setWinchPositionAuto(bool doit)
 207 {
 208     static bool lastState=false;
 209     if(!_state)
 210         return;
 211     if (!doit)
 212     {
 213         lastState=false;
 214         return;
 215     }
 216     if(lastState)
 217         return;
 218     lastState=true;
 219     float lWinchPos[3];
 220     // The ground plane transformed to the local frame.
 221     float ground[4];
 222     _state->planeGlobalToLocal(_global_ground, ground);
 223
 224     float help[3];
 225     //find a normalized vector pointing forward parallel to the ground
 226     help[0]=0;
 227     help[1]=1;
 228     help[2]=0;
 229     Math::cross3(ground,help,help);
 230     //multiplay by initial tow length;
 231     //reduced by 1m to be able to close the
 232     //hitch either if the glider slips backwards a bit
 233     Math::mul3((_winchInitialTowLength-1.),help,help);
 234     //add to the actual pos
 235     Math::add3(_pos,help,lWinchPos);
 236     //put it onto the ground plane
 237     Math::mul3(ground[3],ground,help);
 238     Math::add3(lWinchPos,help,lWinchPos);
 239
 240     _state->posLocalToGlobal(lWinchPos,_winchPos);
 241     _towLength=_winchInitialTowLength;
 242     fgSetString("/sim/messages/pilot", "Connected to winch!");
 243     _open=false;
 244
 245     _node->setBoolValue("broken",false);
 246
 247     //set the dist value (if not, the hitch would open in the next calcforce run
 248     float delta[3];
 249     Math::sub3(lWinchPos,_pos,delta);
 250     _dist=Math::mag3(delta);
 251 }
 252
 253 void Hitch::findBestAIObject(bool doit,bool running_as_autoconnect)
 254 {
 255     static bool lastState=false;
 256     if(!_state)
 257         return;
 258     if (!running_as_autoconnect)
 259     {
 260         //if this function is binded to an input, it will be called every frame as long as the key is pressed.
 261         //therefore wait for a key-release before running it again.
 262         if (!doit)
 263         {
 264             lastState=false;
 265             return;
 266         }
 267         if(lastState)
 268             return;
 269         lastState=true;
 270     }
 271     double gpos[3];
 272     _state->posLocalToGlobal(_pos,gpos);
 273     double bestdist=_towLength*_towLength;//squared!
 274     _towEndIsConnectedToProperty=false;
 275     SGPropertyNode * ainode = fgGetNode("/ai/models",false);
 276     if(!ainode) return;
 277     char myCallsign[256]="***********";
 278     if (running_as_autoconnect)
 279     {
 280         //get own callsign
 281         SGPropertyNode *cs=fgGetNode("/sim/multiplay/callsign",false);
 282         if (cs)
 283         {
 284             strncpy(myCallsign,cs->getStringValue(),256);
 285             myCallsign[255]=0;
 286         }
 287         //reset tow length for search radius. Lentgh will be later copied from master
 288         _towLength=_winchInitialTowLength;
 289     }
 290     bool found=false;
 291     vector <SGPropertyNode_ptr> nodes;//<SGPropertyNode_ptr>
 292     for (int i=0;i<ainode->nChildren();i++)
 293     {
 294         SGPropertyNode * n=ainode->getChild(i);
 295         _nodeIsMultiplayer = strncmp("multiplayer",n->getName(),11)==0;
 296         _nodeIsAiAircraft = strncmp("aircraft",n->getName(),8)==0;
 297         if (!(_nodeIsMultiplayer || _nodeIsAiAircraft))
 298             continue;
 299         if (running_as_autoconnect)
 300         {
 301             if (!_nodeIsMultiplayer)
 302                 continue;
 303             if(n->getBoolValue("sim/hitches/aerotow/open",true)) continue;
 304             if(strncmp(myCallsign,n->getStringValue("sim/hitches/aerotow/tow/connected-to-ai-or-mp-callsign"),255)!=0)
 305                 continue;
 306         }
 307         double pos[3];
 308         pos[0]=n->getDoubleValue("position/global-x",0);
 309         pos[1]=n->getDoubleValue("position/global-y",0);
 310         pos[2]=n->getDoubleValue("position/global-z",0);
 311         double dist=0;
 312         for (int j=0;j<3;j++)
 313             dist+=(pos[j]-gpos[j])*(pos[j]-gpos[j]);
 314         if (dist<bestdist)
 315         {
 316             bestdist=dist;
 317             _towEndNode=n;
 318             _towEndIsConnectedToProperty=true;
 319             //_node->setStringValue("tow/node",n->getPath());
 320             _node->setStringValue("tow/node",n->getDisplayName());
 321             _nodeID=n->getIntValue("id",0);
 322             _node->setStringValue("tow/connected-to-ai-or-mp-callsign",n->getStringValue("callsign"));
 323             _open=false;
 324             found = true;
 325         }
 326     }
 327     if (found)
 328     {
 329         if (!running_as_autoconnect)
 330         {
 331             std::stringstream message;
 332             message<<_node->getStringValue("tow/connected-to-ai-or-mp-callsign")
 333                     <<", I am on your hook, distance "<<Math::sqrt(bestdist)<<" meter.";
 334             fgSetString("/sim/messages/pilot", message.str().c_str());
 335         }
 336         else
 337         {
 338             std::stringstream message;
 339             message<<_node->getStringValue("tow/connected-to-ai-or-mp-callsign")
 340                 <<": I am on your hook, distance "<<Math::sqrt(bestdist)<<" meter.";
 341             fgSetString("/sim/messages/ai-plane", message.str().c_str());
 342         }
 343         if (running_as_autoconnect)
 344             _isSlave=true;
 345         //set the dist value to some value below the tow lentgh (if not, the hitch
 346         //would open in the next calc force run
 347         _dist=_towLength*0.5;
 348         _mp_open_last_state=true;
 349     }
 350     else
 351         if (!running_as_autoconnect)
 352         {
 353             fgSetString("/sim/messages/atc", "Sorry, no aircraft for aerotow!");
 354         }
 355 }
 356
 357 void Hitch::setWinchInitialTowLength(float length)
 358 {
 359     _winchInitialTowLength=length;
 360 }
 361
 362 void Hitch::setWinchPower(float power)
 363 {
 364     _winchPower=power;
 365 }
 366
 367 void Hitch::setWinchMaxTowLength(float length)
 368 {
 369     _winchMaxTowLength=length;
 370 }
 371
 372 void Hitch::setWinchMinTowLength(float length)
 373 {
 374     _winchMinTowLength=length;
 375 }
 376
 377 void Hitch::setGlobalGround(double *global_ground, float *global_vel)
 378 {
 379     int i;
 380     for(i=0; i<4; i++) _global_ground[i] = global_ground[i];
 381     for(i=0; i<3; i++) _global_vel[i] = global_vel[i];
 382 }
 383
 384 void Hitch::getPosition(float* out)
 385 {
 386     int i;
 387     for(i=0; i<3; i++) out[i] = _pos[i];
 388 }
 389
 390 float Hitch::getTowLength(void)
 391 {
 392     return _towLength;
 393 }
 394
 395 void Hitch::calcForce(Ground *g_cb, RigidBody* body, State* s)
 396 {
 397     float lWinchPos[3],delta[3],deltaN[3];
 398     *_state=*s;
 399     s->posGlobalToLocal(_winchPos,lWinchPos);
 400     Math::sub3(lWinchPos,_pos,delta);
 401     if(!_towEndIsConnectedToProperty)
 402         _dist=Math::mag3(delta);
 403     else
 404         _dist=Math::mag3(lWinchPos); //use the aircraft center as reference for distance calculation
 405                                   //this position is transferred to the MP-Aircraft.
 406                                   //With this trick, both player in aerotow get the same length
 407     Math::unit3(delta,deltaN);
 408     float lvel[3];
 409     s->velGlobalToLocal(s->v,lvel);
 410     _speed_in_tow_direction=Math::dot3(lvel,deltaN);
 411     if (_towEndIsConnectedToProperty && _nodeIsMultiplayer)
 412     {
 413         float mp_delta_dist_due_to_time_lag=0.5*_mp_time_lag*(-_mp_v+_speed_in_tow_direction);
 414         _timeLagCorrectedDist=_dist+mp_delta_dist_due_to_time_lag;
 415         if(_forceIsCalculatedByMaster && !_open)
 416         {
 417             s->velGlobalToLocal(_mp_force,_force);
 418             return;
 419         }
 420     }
 421     else
 422         _timeLagCorrectedDist=_dist;
 423     if (_open)
 424     {
 425         _force[0]=_force[1]=_force[2]=0;
 426         return;
 427     }
 428     if(_dist>_towLength)
 429         if(_towLength>1e-3)
 430             _forceMagnitude=(_dist-_towLength)/_towLength*_towElasticConstant;
 431         else
 432             _forceMagnitude=2*_towBrakeForce;
 433     else
 434         _forceMagnitude=0;
 435     if(_forceMagnitude>=_towBrakeForce)
 436     {
 437         _forceMagnitude=0;
 438         _open=true;
 439         _node->setBoolValue("broken",true);
 440         _force[0]=_force[1]=_force[2]=0;
 441         _towEndForce[0]=_towEndForce[1]=_towEndForce[2]=0;
 442         _reportTowEndForce[0]=_reportTowEndForce[1]=_reportTowEndForce[2]=0;
 443         return;
 444     }
 445     Math::mul3(_forceMagnitude,deltaN,_force);
 446     Math::mul3(-1.,_force,_towEndForce);
 447     _winchActualForce=_forceMagnitude; //missing: gravity on this end and friction
 448     //Add the gravitiy of the rope.
 449     //calculate some numbers:
 450     float grav_force=_towWeightPerM*_towLength*9.81;
 451     //the length of the gravity-expanded row:
 452     float leng=_towLength+grav_force*_towLength/_towElasticConstant;
 453     // The ground plane transformed to the local frame.
 454     float ground[4];
 455     s->planeGlobalToLocal(_global_ground, ground);
 456
 457     // The velocity of the contact patch transformed to local coordinates.
 458     //float glvel[3];
 459     //s->velGlobalToLocal(_global_vel, glvel);
 460
 461     _height_above_ground = ground[3] - Math::dot3(_pos, ground);
 462
 463     //the same for the winch-pos (the pos of the tow end)
 464     _winch_height_above_ground = ground[3] - Math::dot3(lWinchPos, ground);
 465
 466     //the frac of the grav force acting on _pos:
 467     float grav_frac=0.5*(1+(_height_above_ground-_winch_height_above_ground)/leng);
 468     grav_frac=Math::clamp(grav_frac,0,1);
 469     float grav_frac_tow_end=1-grav_frac;
 470     //reduce grav_frac, if the tow has ground contact.
 471     if (_height_above_ground<leng) //if not, the tow can not be on ground
 472     {
 473         float fa[3],fb[3],fg[3];
 474         //the grav force an the hitch position:
 475         Math::mul3(-grav_frac*grav_force,ground,fg);
 476         //the total force on hitch postion:
 477         Math::add3(fg,_force,fa);
 478         //the grav force an the tow end position:
 479         Math::mul3(-(1-grav_frac)*grav_force,ground,fg);
 480         //the total force on tow end postion:
 481         //note: sub: _force on tow-end is negative of force on hitch postion
 482         Math::sub3(fg,_force,fb);
 483         float fa_=Math::mag3(fa);
 484         float fb_=Math::mag3(fb);
 485         float stretchedTowLen;
 486         stretchedTowLen=_towLength*(1.+(fa_+fb_)/(2*_towElasticConstant));
 487         //the relative position of the lowest postion of the tow:
 488         if ((fa_+fb_)>1e-3)
 489             _loPosFrac=fa_/(fa_+fb_);
 490         else
 491             _loPosFrac=0.5;
 492         //dist to tow-end parallel to ground
 493         float ground_dist;
 494         float help[3];
 495         //Math::cross3(delta,ground,help);//as long as we calculate the dist without _pos, od it with lWinchpos, the dist to our center....
 496         Math::cross3(lWinchPos,ground,help);
 497         ground_dist=Math::mag3(help);
 498         //height of lowest tow pos (relative to _pos)
 499         _lowest_tow_height=_loPosFrac*Math::sqrt(Math::abs(stretchedTowLen*stretchedTowLen-ground_dist*ground_dist));
 500         if (_height_above_ground<_lowest_tow_height)
 501         {
 502             if (_height_above_ground>1e-3)
 503                 grav_frac*=_height_above_ground/_lowest_tow_height;
 504             else
 505                 grav_frac=0;
 506         }
 507         if (_winch_height_above_ground<(_lowest_tow_height-_height_above_ground+_winch_height_above_ground))
 508         {
 509             if (_winch_height_above_ground>1e-3)
 510                 grav_frac_tow_end*=_winch_height_above_ground/
 511                     (_lowest_tow_height-_height_above_ground+_winch_height_above_ground);
 512             else
 513                 grav_frac_tow_end=0;
 514         }
 515     }
 516     else _lowest_tow_height=_loPosFrac=-1; //for debug output
 517     float grav_force_v[3];
 518     Math::mul3(grav_frac*grav_force,ground,grav_force_v);
 519     Math::add3(grav_force_v,_force,_force);
 520     _forceMagnitude=Math::mag3(_force);
 521     //the same for the tow end:
 522     Math::mul3(grav_frac_tow_end*grav_force,ground,grav_force_v);
 523     Math::add3(grav_force_v,_towEndForce,_towEndForce);
 524     s->velLocalToGlobal(_towEndForce,_towEndForce);
 525
 526     if(_forceMagnitude>=_towBrakeForce)
 527     {
 528         _forceMagnitude=0;
 529         _open=true;
 530         _node->setBoolValue("broken",true);
 531         _force[0]=_force[1]=_force[2]=0;
 532         _towEndForce[0]=_towEndForce[1]=_towEndForce[2]=0;
 533     }
 534
 535
 536 }
 537
 538 // Computed values: total force
 539 void Hitch::getForce(float* force, float* off)
 540 {
 541     Math::set3(_force, force);
 542     Math::set3(_pos, off);
 543 }
 544
 545 void Hitch::integrate (float dt)
 546 {
 547     //check if hitch has opened or closed, if yes: message
 548     if (_open !=_oldOpen)
 549     {
 550         if (_oldOpen)
 551         {
 552             if (_dist>_towLength*1.00001)
 553             {
 554                 std::stringstream message;
 555                 message<<"Could not lock hitch (tow length is insufficient) on hitch "
 556                        <<_node->getName()<<" "<<_node->getIndex()<<"!";
 557                 fgSetString("/sim/messages/pilot", message.str().c_str());
 558                 _open=true;
 559                 return;
 560             }
 561             _node->setBoolValue("broken",false);
 562         }
 563         std::stringstream message;
 564         if (_node->getBoolValue("broken",false)&&_open)
 565             message<<"Oh no, the tow is broken";
 566         else
 567             message<<(_open?"Opened hitch ":"Locked hitch ")<<_node->getName()<<" "<<_node->getIndex()<<"!";
 568         fgSetString("/sim/messages/pilot", message.str().c_str());
 569         _oldOpen=_open;
 570     }
 571
 572     //check, if tow end should be searched in all MP-aircrafts
 573     if(_open && _mpAutoConnectPeriod)
 574     {
 575         _isSlave=false;
 576         _timeToNextAutoConnectTry-=dt;
 577         if ((_timeToNextAutoConnectTry>_mpAutoConnectPeriod) || (_timeToNextAutoConnectTry<0))
 578         {
 579             _timeToNextAutoConnectTry=_mpAutoConnectPeriod;
 580             //search for MP-Aircraft, which is towed with us
 581             findBestAIObject(true,true);
 582         }
 583     }
 584     //check, if tow end can be modified by property, if yes: update
 585     if(_towEndIsConnectedToProperty)
 586     {
 587         if (_node)
 588         {
 589             //_towEndNode=fgGetNode(_node->getStringValue("tow/node"), false);
 590             char towNode[256];
 591             strncpy(towNode,_node->getStringValue("tow/node"),256);
 592             towNode[255]=0;
 593             _towEndNode=fgGetNode("ai/models")->getNode(towNode, false);
 594             //AI and multiplayer objects seem to change node?
 595             //Check if we have the right one by callsign
 596             if (_nodeIsMultiplayer || _nodeIsAiAircraft)
 597             {
 598                 char MPcallsign[256]="";
 599                 const char *MPc;
 600                 MPc=_node->getStringValue("tow/connected-to-ai-or-mp-callsign");
 601                 if (MPc)
 602                 {
 603                     strncpy(MPcallsign,MPc,256);
 604                     MPcallsign[255]=0;
 605                 }
 606                 if (((_towEndNode)&&(strncmp(_towEndNode->getStringValue("callsign"),MPcallsign,255)!=0))||!_towEndNode)
 607                 {
 608                     _timeToNextReConnectTry-=dt;
 609                     if((_timeToNextReConnectTry<0)||(_timeToNextReConnectTry>10))
 610                     {
 611                         _timeToNextReConnectTry=10;
 612                         SGPropertyNode * ainode = fgGetNode("/ai/models",false);
 613                         if(ainode)
 614                         {
 615                             for (int i=0;i<ainode->nChildren();i++)
 616                             {
 617                                 SGPropertyNode * n=ainode->getChild(i);
 618                                 if(_nodeIsMultiplayer?strncmp("multiplayer",n->getName(),11)==0:strncmp("aircraft",n->getName(),8))
 619                                     if (strcmp(n->getStringValue("callsign"),MPcallsign)==0)//found
 620                                     {
 621                                         _towEndNode=n;
 622                                         //_node->setStringValue("tow/node",n->getPath());
 623                                         _node->setStringValue("tow/node",n->getDisplayName());
 624                                     }
 625                             }
 626                         }
 627                     }
 628                 }
 629             }
 630             if(_towEndNode)
 631             {
 632                 _winchPos[0]=_towEndNode->getDoubleValue("position/global-x",_winchPos[0]);
 633                 _winchPos[1]=_towEndNode->getDoubleValue("position/global-y",_winchPos[1]);
 634                 _winchPos[2]=_towEndNode->getDoubleValue("position/global-z",_winchPos[2]);
 635                 _mp_lpos[0]=_towEndNode->getFloatValue("sim/hitches/aerotow/local-pos-x",0);
 636                 _mp_lpos[1]=_towEndNode->getFloatValue("sim/hitches/aerotow/local-pos-y",0);
 637                 _mp_lpos[2]=_towEndNode->getFloatValue("sim/hitches/aerotow/local-pos-z",0);
 638                 _mp_dist=_towEndNode->getFloatValue("sim/hitches/aerotow/tow/dist");
 639                 _mp_v=_towEndNode->getFloatValue("sim/hitches/aerotow/speed-in-tow-direction");
 640                 _mp_force[0]=_towEndNode->getFloatValue("sim/hitches/aerotow/tow/end-force-x",0);
 641                 _mp_force[1]=_towEndNode->getFloatValue("sim/hitches/aerotow/tow/end-force-y",0);
 642                 _mp_force[2]=_towEndNode->getFloatValue("sim/hitches/aerotow/tow/end-force-z",0);
 643
 644                 if(_isSlave)
 645                 {
 646 #define gf(a,b) a=_towEndNode->getFloatValue(b,a)
 647 #define gb(a,b) a=_towEndNode->getBoolValue(b,a)
 648                     gf(_towLength,"sim/hitches/aerotow/tow/length");
 649                     gf(_towElasticConstant,"sim/hitches/aerotow/tow/elastic-constant");
 650                     gf(_towWeightPerM,"sim/hitches/aerotow/tow/weight-per-m-kg-m");
 651                     gf(_towBrakeForce,"sim/hitches/aerotow/brake-force");
 652                     gb(_open,"sim/hitches/aerotow/open");
 653                     gb(_mp_is_slave,"sim/hitches/aerotow/is-slave");
 654 #undef gf
 655 #undef gb
 656                     if (_mp_is_slave) _isSlave=false; //someone should be master
 657                 }
 658                 else
 659                 {
 660                     //check if other has opened hitch, but is neccessary, that it was closed before
 661                     bool mp_open=_towEndNode->getBoolValue("sim/hitches/aerotow/open",_mp_open_last_state);
 662                     if (mp_open != _mp_open_last_state) //state has changed
 663                     {
 664                         _mp_open_last_state=mp_open; //store that value
 665                         if(!_open)
 666                         {
 667                             if(mp_open)
 668                             {
 669                                 _oldOpen=_open=true;
 670                                 std::stringstream message;
 671                                 message<<_node->getStringValue("tow/connected-to-ai-or-mp-callsign")
 672                                     <<": I have released the tow!";
 673                                 fgSetString("/sim/messages/ai-plane", message.str().c_str());
 674                             }
 675                         }
 676                     }
 677                 }
 678                 //try to calculate the time lag
 679                 if ((_mp_last_reported_dist!=_mp_dist)||(_mp_last_reported_v!=_mp_v)) //new data;
 680                 {
 681                     _mp_last_reported_dist=_mp_dist;
 682                     _mp_last_reported_v=_mp_v;
 683                     float total_v=-_mp_v+_speed_in_tow_direction;//mp has opposite tow direction
 684                     float abs_v=Math::abs(total_v);
 685                     if (abs_v>0.1)
 686                     {
 687                         float actual_time_lag_guess=(_mp_dist-_dist)/total_v;
 688                         //check, if it sounds ok
 689                         if((actual_time_lag_guess>0)&&(actual_time_lag_guess<5))
 690                         {
 691                             float frac=abs_v*0.01;
 692                             if (frac>0.05) frac=0.05;
 693                             // if we are slow, the guess of the lag can be rather wrong. as faster we are
 694                             // the better the guess. Therefore frac is proportiona to the speed. Clamp it
 695                             // at 5m/s
 696                             _mp_time_lag=(1-frac)*_mp_time_lag+frac*actual_time_lag_guess;
 697                         }
 698                     }
 699                 }
 700             }
 701         }
 702     }
 703     //set the _reported_tow_end_force
 704     Math::set3(_towEndForce,_reportTowEndForce);
 705
 706     if (_open) return;
 707     if (_winchRelSpeed==0) return;
 708     float factor=1,offset=0;
 709     if (_winchActualForce>_winchMaxForce)
 710         offset=-(_winchActualForce-_winchMaxForce)/_winchMaxForce*20;
 711     if (_winchRelSpeed>0.01) // to avoit div by zero
 712     {
 713         float maxForcePowerLimit=_winchPower/(_winchRelSpeed*_winchMaxSpeed);
 714         if (_winchActualForce>maxForcePowerLimit)
 715             factor-=(_winchActualForce-maxForcePowerLimit)/maxForcePowerLimit;
 716     }
 717     _towLength-=dt*(factor*_winchRelSpeed+offset)*_winchMaxSpeed;
 718     if (_towLength<=_winchMinTowLength)
 719     {
 720         if (_winchRelSpeed<0)
 721             _winchRelSpeed=0;
 722         _towLength=_winchMinTowLength;
 723         return;
 724     }
 725     if (_towLength>=_winchMaxTowLength)
 726     {
 727         if (_winchRelSpeed<0)
 728             _winchRelSpeed=0;
 729         _towLength=_winchMaxTowLength;
 730         return;
 731     }
 732 }
 733
 734
 735
 736
 737 }; // namespace yasim