2 #include "BodyEnvironment.hpp"
3 #include "RigidBody.hpp"
4 #include <Main/fg_props.hxx>
11 Hitch::Hitch(const char *name)
14 strncpy(_name,name,128);
17 _pos[i] = _force[i] = _winchPos[i] = _mp_lpos[i]=_towEndForce[i]=_mp_force[i]=0;
19 _global_ground[i] = 0;
20 _global_ground[2] = 1;
21 _global_ground[3] = -1e5;
26 _towElasticConstant=1e5;
27 _towBrakeForce=100000;
31 _winchInitialTowLength=1000;
35 _winchMaxTowLength=1000;
38 _towEndIsConnectedToProperty=false;
40 _nodeIsMultiplayer=false;
41 _nodeIsAiAircraft=false;
42 _forceIsCalculatedByMaster=false;
45 _height_above_ground=0;
46 _winch_height_above_ground=0;
50 _displayed_len_lower_dist_message=false;
53 _mpAutoConnectPeriod=0;
54 _timeToNextAutoConnectTry=0;
55 _timeToNextReConnectTry=10;
56 _speed_in_tow_direction=0;
58 _mp_last_reported_dist=0;
59 _mp_last_reported_v=0;
61 _mp_open_last_state=false;
62 _timeLagCorrectedDist=0;
66 sprintf(text,"/sim/hitches/%s", _name);
67 SGPropertyNode * node = fgGetNode(text, true);
68 node->tie("tow/length",SGRawValuePointer<float>(&_towLength));
69 node->tie("tow/elastic-constant",SGRawValuePointer<float>(&_towElasticConstant));
70 node->tie("tow/weight-per-m-kg-m",SGRawValuePointer<float>(&_towWeightPerM));
71 node->tie("tow/brake-force",SGRawValuePointer<float>(&_towBrakeForce));
72 node->tie("winch/max-speed-m-s",SGRawValuePointer<float>(&_winchMaxSpeed));
73 node->tie("winch/rel-speed",SGRawValuePointer<float>(&_winchRelSpeed));
74 node->tie("winch/initial-tow-length-m",SGRawValuePointer<float>(&_winchInitialTowLength));
75 node->tie("winch/min-tow-length-m",SGRawValuePointer<float>(&_winchMinTowLength));
76 node->tie("winch/max-tow-length-m",SGRawValuePointer<float>(&_winchMaxTowLength));
77 node->tie("winch/global-pos-x",SGRawValuePointer<double>(&_winchPos[0]));
78 node->tie("winch/global-pos-y",SGRawValuePointer<double>(&_winchPos[1]));
79 node->tie("winch/global-pos-z",SGRawValuePointer<double>(&_winchPos[2]));
80 node->tie("winch/max-power",SGRawValuePointer<float>(&_winchPower));
81 node->tie("winch/max-force",SGRawValuePointer<float>(&_winchMaxForce));
82 node->tie("winch/actual-force",SGRawValuePointer<float>(&_winchActualForce));
83 node->tie("tow/end-force-x",SGRawValuePointer<float>(&_reportTowEndForce[0]));
84 node->tie("tow/end-force-y",SGRawValuePointer<float>(&_reportTowEndForce[1]));
85 node->tie("tow/end-force-z",SGRawValuePointer<float>(&_reportTowEndForce[2]));
86 node->tie("force",SGRawValuePointer<float>(&_forceMagnitude));
87 node->tie("open",SGRawValuePointer<bool>(&_open));
88 node->tie("force-is-calculated-by-other",SGRawValuePointer<bool>(&_forceIsCalculatedByMaster));
89 node->tie("local-pos-x",SGRawValuePointer<float>(&_pos[0]));
90 node->tie("local-pos-y",SGRawValuePointer<float>(&_pos[1]));
91 node->tie("local-pos-z",SGRawValuePointer<float>(&_pos[2]));
92 node->tie("tow/dist",SGRawValuePointer<float>(&_dist));
93 node->tie("tow/dist-time-lag-corrected",SGRawValuePointer<float>(&_timeLagCorrectedDist));
94 node->tie("tow/connected-to-property-node",SGRawValuePointer<bool>(&_towEndIsConnectedToProperty));
95 node->tie("tow/connected-to-mp-node",SGRawValuePointer<bool>(&_nodeIsMultiplayer));
96 node->tie("tow/connected-to-ai-node",SGRawValuePointer<bool>(&_nodeIsAiAircraft));
97 node->tie("tow/connected-to-ai-or-mp-id",SGRawValuePointer<int>(&_nodeID));
98 node->tie("debug/hitch-height-above-ground",SGRawValuePointer<float>(&_height_above_ground));
99 node->tie("debug/tow-end-height-above-ground",SGRawValuePointer<float>(&_winch_height_above_ground));
100 node->tie("debug/tow-rel-lo-pos",SGRawValuePointer<float>(&_loPosFrac));
101 node->tie("debug/tow-lowest-pos-height",SGRawValuePointer<float>(&_lowest_tow_height));
102 node->tie("is-slave",SGRawValuePointer<bool>(&_isSlave));
103 node->tie("speed-in-tow-direction",SGRawValuePointer<float>(&_speed_in_tow_direction));
104 node->tie("mp-auto-connect-period",SGRawValuePointer<float>(&_mpAutoConnectPeriod));
105 node->tie("mp-time-lag",SGRawValuePointer<float>(&_mp_time_lag));
106 node->setStringValue("tow/Node","");
107 node->setStringValue("tow/connectedToAIorMP-callsign");
108 node->setBoolValue("broken",false);
114 sprintf(text,"/sim/hitches/%s", _name);
115 SGPropertyNode * node = fgGetNode(text, true);
116 node->untie("tow/length");
117 node->untie("tow/elastic-constant");
118 node->untie("tow/weight-per-m-kg-m");
119 node->untie("tow/brake-force");
120 node->untie("winch/max-speed-m-s");
121 node->untie("winch/rel-speed");
122 node->untie("winch/initial-tow-length-m");
123 node->untie("winch/min-tow-length-m");
124 node->untie("winch/max-tow-length-m");
125 node->untie("winch/global-pos-x");
126 node->untie("winch/global-pos-y");
127 node->untie("winch/global-pos-z");
128 node->untie("winch/max-power");
129 node->untie("winch/max-force");
130 node->untie("winch/actual-force");
131 node->untie("tow/end-force-x");
132 node->untie("tow/end-force-y");
133 node->untie("tow/end-force-z");
134 node->untie("force");
136 node->untie("force-is-calculated-by-other");
137 node->untie("local-pos-x");
138 node->untie("local-pos-y");
139 node->untie("local-pos-z");
140 node->untie("tow/dist");
141 node->untie("tow/dist-time-lag-corrected");
142 node->untie("tow/connected-to-property-node");
143 node->untie("tow/connected-to-mp-node");
144 node->untie("tow/connected-to-ai-node");
145 node->untie("tow/connected-to-ai-or-mp-id");
146 node->untie("debug/hitch-height-above-ground");
147 node->untie("debug/tow-end-height-above-ground");
148 node->untie("debug/tow-rel-lo-pos");
149 node->untie("debug/tow-lowest-pos-height");
150 node->untie("is-slave");
151 node->untie("speed-in-tow-direction");
152 node->untie("mp-auto-connect-period");
153 node->untie("mp-time-lag");
158 void Hitch::setPosition(float* position)
161 for(i=0; i<3; i++) _pos[i] = position[i];
164 void Hitch::setTowLength(float length)
169 void Hitch::setOpen(bool isOpen)
171 //test if we already processed this before
172 //without this test a binded property could
173 //try to close the Hitch every run
174 //it will close, if we are near the end
175 //e.g. if we are flying over the parked
177 if (isOpen==_last_wish)
183 void Hitch::setTowElasticConstant(float sc)
185 _towElasticConstant=sc;
188 void Hitch::setTowBreakForce(float bf)
193 void Hitch::setWinchMaxForce(float f)
198 void Hitch::setTowWeightPerM(float rw)
203 void Hitch::setWinchMaxSpeed(float mws)
208 void Hitch::setWinchRelSpeed(float rws)
213 void Hitch::setWinchPosition(double *winchPosition)//in global coordinates!
215 for (int i=0; i<3;i++)
216 _winchPos[i]=winchPosition[i];
219 void Hitch::setMpAutoConnectPeriod(float dt)
221 _mpAutoConnectPeriod=dt;
224 void Hitch::setForceIsCalculatedByOther(bool b)
226 _forceIsCalculatedByMaster=b;
229 const char *Hitch::getConnectedPropertyNode() const
232 return _towEndNode->getDisplayName();
237 void Hitch::setConnectedPropertyNode(const char *nodename)
239 _towEndNode=fgGetNode(nodename,false);
242 void Hitch::setWinchPositionAuto(bool doit)
244 static bool lastState=false;
256 // The ground plane transformed to the local frame.
258 _state->planeGlobalToLocal(_global_ground, ground);
261 //find a normalized vector pointing forward parallel to the ground
265 Math::cross3(ground,help,help);
266 //multiplay by initial tow length;
267 //reduced by 1m to be able to close the
268 //hitch either if the glider slips backwards a bit
269 Math::mul3((_winchInitialTowLength-1.),help,help);
270 //add to the actual pos
271 Math::add3(_pos,help,lWinchPos);
272 //put it onto the ground plane
273 Math::mul3(ground[3],ground,help);
274 Math::add3(lWinchPos,help,lWinchPos);
276 _state->posLocalToGlobal(lWinchPos,_winchPos);
277 _towLength=_winchInitialTowLength;
278 SG_LOG(SG_GENERAL, SG_ALERT,"Set the winch pos to "<<_winchPos[0]<<","<<_winchPos[1]<<","<<_winchPos[2]<<endl);
282 sprintf(text,"/sim/hitches/%s", _name);
283 SGPropertyNode * node = fgGetNode(text, true);
284 node->setBoolValue("broken",false);
286 //set the dist value (if not, the hitch would open in the next calcforce run
288 //Math::sub3(lWinchPos,_pos,delta);
289 //_dist=Math::mag3(delta);
290 _dist=Math::mag3(lWinchPos); //use the aircraft center as reference for distance calculation
291 //this position is transferred to the MP-Aircraft.
292 //With this trick, both player in aerotow get the same length
296 void Hitch::findBestAIObject(bool doit,bool running_as_autoconnect)
298 static bool lastState=false;
310 _state->posLocalToGlobal(_pos,gpos);
311 double bestdist=_towLength*_towLength;//squared!
312 _towEndIsConnectedToProperty=false;
313 SGPropertyNode * ainode = fgGetNode("/ai/models",false);
315 char myCallsign[256]="***********";
316 if (running_as_autoconnect)
319 SGPropertyNode *cs=fgGetNode("/sim/multiplay/callsign",false);
322 strncpy(myCallsign,cs->getStringValue(),256);
325 //reset tow length for search radius. Lentgh will be later copied from master
326 _towLength=_winchInitialTowLength;
329 vector <SGPropertyNode_ptr> nodes;//<SGPropertyNode_ptr>
330 for (int i=0;i<ainode->nChildren();i++)
332 SGPropertyNode * n=ainode->getChild(i);
333 _nodeIsMultiplayer = strncmp("multiplayer",n->getName(),11)==0;
334 _nodeIsAiAircraft = strncmp("aircraft",n->getName(),8)==0;
335 if (!(_nodeIsMultiplayer || _nodeIsAiAircraft))
337 if (running_as_autoconnect)
339 if (!_nodeIsMultiplayer)
341 if(n->getBoolValue("sim/hitches/aerotow/open",true)) continue;
342 if(strncmp(myCallsign,n->getStringValue("sim/hitches/aerotow/tow/connectedToAIorMP-callsign"),255)!=0)
346 pos[0]=n->getDoubleValue("position/global-x",0);
347 pos[1]=n->getDoubleValue("position/global-y",0);
348 pos[2]=n->getDoubleValue("position/global-z",0);
350 for (int j=0;j<3;j++)
351 dist+=(pos[j]-gpos[j])*(pos[j]-gpos[j]);
356 _towEndIsConnectedToProperty=true;
358 sprintf(text,"/sim/hitches/%s", _name);
359 SGPropertyNode * node = fgGetNode(text, true);
360 //node->setStringValue("tow/Node",n->getPath());
361 node->setStringValue("tow/Node",n->getDisplayName());
362 _nodeID=n->getIntValue("id",0);
363 node->setStringValue("tow/connectedToAIorMP-callsign",n->getStringValue("callsign"));
371 sprintf(text,"/sim/hitches/%s", _name);
372 SGPropertyNode * node = fgGetNode(text, true);
373 //if (!running_as_autoconnect)
374 SG_LOG(SG_GENERAL, SG_ALERT,"Found aircraft for aerotow: "
375 <<node->getStringValue("tow/connectedToAIorMP-callsign")
376 <<", distance "<<Math::sqrt(bestdist)<<"m at "
377 <<node->getStringValue("tow/Node")<<endl);
378 if (running_as_autoconnect)
380 //set the dist value to some value below the tow lentgh (if not, the hitch
381 //would open in the next calc force run
382 _dist=_towLength*0.5;
383 _mp_open_last_state=true;
386 if (!running_as_autoconnect)
387 SG_LOG(SG_GENERAL, SG_ALERT,"Found no aircraft for aerotow!"<<endl);
390 void Hitch::setWinchInitialTowLength(float length)
392 _winchInitialTowLength=length;
395 void Hitch::setWinchPower(float power)
400 void Hitch::setWinchMaxTowLength(float length)
402 _winchMaxTowLength=length;
405 void Hitch::setWinchMinTowLength(float length)
407 _winchMinTowLength=length;
410 void Hitch::setGlobalGround(double *global_ground, float *global_vel)
413 for(i=0; i<4; i++) _global_ground[i] = global_ground[i];
414 for(i=0; i<3; i++) _global_vel[i] = global_vel[i];
417 void Hitch::getPosition(float* out)
420 for(i=0; i<3; i++) out[i] = _pos[i];
423 float Hitch::getTowLength(void)
428 void Hitch::calcForce(Ground *g_cb, RigidBody* body, State* s)
430 float lWinchPos[3],delta[3],deltaN[3];
432 s->posGlobalToLocal(_winchPos,lWinchPos);
433 Math::sub3(lWinchPos,_pos,delta);
434 //_dist=Math::mag3(delta);
435 _dist=Math::mag3(lWinchPos); //use the aircraft center as reference for distance calculation
436 //this position is transferred to the MP-Aircraft.
437 //With this trick, both player in aerotow get the same length
438 Math::unit3(delta,deltaN);
440 s->velGlobalToLocal(s->v,lvel);
441 _speed_in_tow_direction=Math::dot3(lvel,deltaN);
442 if (_towEndIsConnectedToProperty && _nodeIsMultiplayer)
444 float mp_delta_dist_due_to_time_lag=0.5*_mp_time_lag*(-_mp_v+_speed_in_tow_direction);
445 _timeLagCorrectedDist=_dist+mp_delta_dist_due_to_time_lag;
446 if(_forceIsCalculatedByMaster && !_open)
448 s->velGlobalToLocal(_mp_force,_force);
453 _timeLagCorrectedDist=_dist;
456 _force[0]=_force[1]=_force[2]=0;
461 _forceMagnitude=(_dist-_towLength)/_towLength*_towElasticConstant;
463 _forceMagnitude=2*_towBrakeForce;
466 if(_forceMagnitude>=_towBrakeForce)
471 sprintf(text,"/sim/hitches/%s", _name);
472 SGPropertyNode * node = fgGetNode(text, true);
473 node->setBoolValue("broken",true);
474 _force[0]=_force[1]=_force[2]=0;
475 _towEndForce[0]=_towEndForce[1]=_towEndForce[2]=0;
476 _reportTowEndForce[0]=_reportTowEndForce[1]=_reportTowEndForce[2]=0;
479 Math::mul3(_forceMagnitude,deltaN,_force);
480 Math::mul3(-1.,_force,_towEndForce);
481 _winchActualForce=_forceMagnitude; //missing: gravity on this end and friction
482 //Add the gravitiy of the rope.
483 //calculate some numbers:
484 float grav_force=_towWeightPerM*_towLength*9.81;
485 //the length of the gravity-expanded row:
486 float leng=_towLength+grav_force*_towLength/_towElasticConstant;
487 // The ground plane transformed to the local frame.
489 s->planeGlobalToLocal(_global_ground, ground);
491 // The velocity of the contact patch transformed to local coordinates.
493 //s->velGlobalToLocal(_global_vel, glvel);
495 _height_above_ground = ground[3] - Math::dot3(_pos, ground);
497 //the same for the winch-pos (the pos of the tow end)
498 _winch_height_above_ground = ground[3] - Math::dot3(lWinchPos, ground);
500 //the frac of the grav force acting on _pos:
501 float grav_frac=0.5*(1+(_height_above_ground-_winch_height_above_ground)/leng);
502 grav_frac=Math::clamp(grav_frac,0,1);
503 float grav_frac_tow_end=1-grav_frac;
504 //reduce grav_frac, if the tow has ground contact.
505 if (_height_above_ground<leng) //if not, the tow can not be on ground
507 float fa[3],fb[3],fg[3];
508 //the grav force an the hitch position:
509 Math::mul3(-grav_frac*grav_force,ground,fg);
510 //the total force on hitch postion:
511 Math::add3(fg,_force,fa);
512 //the grav force an the tow end position:
513 Math::mul3(-(1-grav_frac)*grav_force,ground,fg);
514 //the total force on tow end postion:
515 //note: sub: _force on tow-end is negative of force on hitch postion
516 Math::sub3(fg,_force,fb);
517 float fa_=Math::mag3(fa);
518 float fb_=Math::mag3(fb);
519 float stretchedTowLen;
520 stretchedTowLen=_towLength*(1.+(fa_+fb_)/(2*_towElasticConstant));
521 //the relative position of the lowest postion of the tow:
523 _loPosFrac=fa_/(fa_+fb_);
526 //dist to tow-end parallel to ground
529 //Math::cross3(delta,ground,help);//as long as we calculate the dist without _pos, od it with lWinchpos, the dist to our center....
530 Math::cross3(lWinchPos,ground,help);
531 ground_dist=Math::mag3(help);
532 //height of lowest tow pos (relative to _pos)
533 _lowest_tow_height=_loPosFrac*Math::sqrt(Math::abs(stretchedTowLen*stretchedTowLen-ground_dist*ground_dist));
534 if (_height_above_ground<_lowest_tow_height)
536 if (_height_above_ground>1e-3)
537 grav_frac*=_height_above_ground/_lowest_tow_height;
541 if (_winch_height_above_ground<(_lowest_tow_height-_height_above_ground+_winch_height_above_ground))
543 if (_winch_height_above_ground>1e-3)
544 grav_frac_tow_end*=_winch_height_above_ground/
545 (_lowest_tow_height-_height_above_ground+_winch_height_above_ground);
550 else _lowest_tow_height=_loPosFrac=-1; //for debug output
551 float grav_force_v[3];
552 Math::mul3(grav_frac*grav_force,ground,grav_force_v);
553 Math::add3(grav_force_v,_force,_force);
554 _forceMagnitude=Math::mag3(_force);
555 //the same for the tow end:
556 Math::mul3(grav_frac_tow_end*grav_force,ground,grav_force_v);
557 Math::add3(grav_force_v,_towEndForce,_towEndForce);
558 s->velLocalToGlobal(_towEndForce,_towEndForce);
560 if(_forceMagnitude>=_towBrakeForce)
565 sprintf(text,"/sim/hitches/%s", _name);
566 SGPropertyNode * node = fgGetNode(text, true);
567 node->setBoolValue("broken",true);
568 _force[0]=_force[1]=_force[2]=0;
569 _towEndForce[0]=_towEndForce[1]=_towEndForce[2]=0;
575 // Computed values: total force
576 void Hitch::getForce(float* force, float* off)
578 Math::set3(_force, force);
579 Math::set3(_pos, off);
582 void Hitch::integrate (float dt)
584 //check if hitch has opened or closed, if yes: message
585 if (_open !=_oldOpen)
589 if (_dist>_towLength*1.00001)
591 SG_LOG(SG_GENERAL, SG_ALERT,"Could not lock Hinch (tow length is insufficient) on hitch '"<<_name<<"' !"<<endl);
596 sprintf(text,"/sim/hitches/%s", _name);
597 SGPropertyNode * node = fgGetNode(text, true);
598 node->setBoolValue("broken",false);
600 SG_LOG(SG_GENERAL, SG_ALERT,(_open?"Opened hitch (or broken tow) '":"Locked hitch '")<<_name<<"' !"<<endl);
604 //check, if tow end should be searched in all MP-aircrafts
605 if(_open && _mpAutoConnectPeriod)
608 _timeToNextAutoConnectTry-=dt;
609 if ((_timeToNextAutoConnectTry>_mpAutoConnectPeriod) || (_timeToNextAutoConnectTry<0))
611 _timeToNextAutoConnectTry=_mpAutoConnectPeriod;
612 //search for MP-Aircraft, which is towed with us
613 findBestAIObject(true,true);
616 //check, if tow end can be modified by property, if yes: update
617 if(_towEndIsConnectedToProperty)
620 sprintf(text,"/sim/hitches/%s", _name);
621 SGPropertyNode * node = fgGetNode(text, true);
624 //_towEndNode=fgGetNode(node->getStringValue("tow/Node"), false);
626 strncpy(towNode,node->getStringValue("tow/Node"),256);
628 _towEndNode=fgGetNode("ai/models")->getNode(towNode, false);
629 //AI and multiplayer objects seem to change node?
630 //Check if we have the right one by callsign
631 if (_nodeIsMultiplayer || _nodeIsAiAircraft)
633 char MPcallsign[256]="";
635 MPc=node->getStringValue("tow/connectedToAIorMP-callsign");
638 strncpy(MPcallsign,MPc,256);
641 if (((_towEndNode)&&(strncmp(_towEndNode->getStringValue("callsign"),MPcallsign,255)!=0))||!_towEndNode)
643 _timeToNextReConnectTry-=dt;
644 if((_timeToNextReConnectTry<0)||(_timeToNextReConnectTry>10))
646 _timeToNextReConnectTry=10;
647 SGPropertyNode * ainode = fgGetNode("/ai/models",false);
650 for (int i=0;i<ainode->nChildren();i++)
652 SGPropertyNode * n=ainode->getChild(i);
653 if(_nodeIsMultiplayer?strncmp("multiplayer",n->getName(),11)==0:strncmp("aircraft",n->getName(),8))
654 if (strcmp(n->getStringValue("callsign"),MPcallsign)==0)//found
657 //node->setStringValue("tow/Node",n->getPath());
658 node->setStringValue("tow/Node",n->getDisplayName());
667 _winchPos[0]=_towEndNode->getDoubleValue("position/global-x",_winchPos[0]);
668 _winchPos[1]=_towEndNode->getDoubleValue("position/global-y",_winchPos[1]);
669 _winchPos[2]=_towEndNode->getDoubleValue("position/global-z",_winchPos[2]);
670 _mp_lpos[0]=_towEndNode->getFloatValue("sim/hitches/aerotow/local-pos-x",0);
671 _mp_lpos[1]=_towEndNode->getFloatValue("sim/hitches/aerotow/local-pos-y",0);
672 _mp_lpos[2]=_towEndNode->getFloatValue("sim/hitches/aerotow/local-pos-z",0);
673 _mp_dist=_towEndNode->getFloatValue("sim/hitches/aerotow/tow/dist");
674 _mp_v=_towEndNode->getFloatValue("sim/hitches/aerotow/speed-in-tow-direction");
675 _mp_force[0]=_towEndNode->getFloatValue("sim/hitches/aerotow/tow/end-force-x",0);
676 _mp_force[1]=_towEndNode->getFloatValue("sim/hitches/aerotow/tow/end-force-y",0);
677 _mp_force[2]=_towEndNode->getFloatValue("sim/hitches/aerotow/tow/end-force-z",0);
681 #define gf(a,b) a=_towEndNode->getFloatValue(b,a)
682 #define gb(a,b) a=_towEndNode->getBoolValue(b,a)
683 gf(_towLength,"sim/hitches/aerotow/tow/length");
684 gf(_towElasticConstant,"sim/hitches/aerotow/tow/elastic-constant");
685 gf(_towWeightPerM,"sim/hitches/aerotow/tow/weight-per-m-kg-m");
686 gf(_towBrakeForce,"sim/hitches/aerotow/brake-force");
687 gb(_open,"sim/hitches/aerotow/open");
688 gb(_mp_is_slave,"sim/hitches/aerotow/is-slave");
691 if (_mp_is_slave) _isSlave=false; //someone should be master
695 //check if other has opened hitch, but is neccessary, that it was closed before
696 bool mp_open=_towEndNode->getBoolValue("sim/hitches/aerotow/open",_mp_open_last_state);
697 if (mp_open != _mp_open_last_state) //state has changed
699 _mp_open_last_state=mp_open; //store that value
706 sprintf(text,"/sim/hitches/%s", _name);
707 SGPropertyNode * node = fgGetNode(text, true);
708 node->getStringValue("tow/Node","");
709 SG_LOG(SG_GENERAL, SG_ALERT,"'"<<node->getStringValue("tow/Node","")<<"' has opened hitch!"<<endl);
714 //try to calculate the time lag
715 if ((_mp_last_reported_dist!=_mp_dist)||(_mp_last_reported_v!=_mp_v)) //new data;
717 _mp_last_reported_dist=_mp_dist;
718 _mp_last_reported_v=_mp_v;
719 float total_v=-_mp_v+_speed_in_tow_direction;//mp has opposite tow direction
720 float abs_v=Math::abs(total_v);
723 float actual_time_lag_guess=(_mp_dist-_dist)/total_v;
724 //check, if it sounds ok
725 if((actual_time_lag_guess>0)&&(actual_time_lag_guess<5))
727 float frac=abs_v*0.01;
728 if (frac>0.05) frac=0.05;
729 // if we are slow, the guess of the lag can be rather wrong. as faster we are
730 // the better the guess. Therefore frac is proportiona to the speed. Clamp it
732 _mp_time_lag=(1-frac)*_mp_time_lag+frac*actual_time_lag_guess;
739 //set the _reported_tow_end_force (smoothed)
740 //smooth it a bit and store it
741 float sc=10.*dt; //100ms
743 Math::mul3(sc,_towEndForce,tmp);
744 Math::mul3(1.-sc,_reportTowEndForce,_reportTowEndForce);
745 Math::add3(tmp,_reportTowEndForce,_reportTowEndForce);
748 if (_winchRelSpeed==0) return;
749 float factor=1,offset=0;
750 if (_winchActualForce>_winchMaxForce)
751 offset=-(_winchActualForce-_winchMaxForce)/_winchMaxForce*20;
752 if (_winchRelSpeed>0.01) // to avoit div by zero
754 float maxForcePowerLimit=_winchPower/(_winchRelSpeed*_winchMaxSpeed);
755 if (_winchActualForce>maxForcePowerLimit)
756 factor-=(_winchActualForce-maxForcePowerLimit)/maxForcePowerLimit;
758 _towLength-=dt*(factor*_winchRelSpeed+offset)*_winchMaxSpeed;
759 if (_towLength<=_winchMinTowLength)
761 if (_winchRelSpeed<0)
763 _towLength=_winchMinTowLength;
766 if (_towLength>=_winchMaxTowLength)
768 if (_winchRelSpeed<0)
770 _towLength=_winchMaxTowLength;
778 }; // namespace yasim