src/FDM/YASim/Hitch.cpp

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