2 #include "BodyEnvironment.hpp"
3 #include "RigidBody.hpp"
11 Hitch::Hitch(const char *name)
13 _node = fgGetNode("/sim/hitches", true)->getNode(name, true);
16 _pos[i] = _force[i] = _winchPos[i] = _mp_lpos[i]=_towEndForce[i]=_mp_force[i]=0;
18 _global_ground[i] = 0;
19 _global_ground[2] = 1;
20 _global_ground[3] = -1e5;
25 _towElasticConstant=1e5;
26 _towBrakeForce=100000;
30 _winchInitialTowLength=1000;
34 _winchMaxTowLength=1000;
37 _towEndIsConnectedToProperty=false;
39 _nodeIsMultiplayer=false;
40 _nodeIsAiAircraft=false;
41 _forceIsCalculatedByMaster=false;
44 _height_above_ground=0;
45 _winch_height_above_ground=0;
49 _displayed_len_lower_dist_message=false;
52 _mpAutoConnectPeriod=0;
53 _timeToNextAutoConnectTry=0;
54 _timeToNextReConnectTry=10;
55 _speed_in_tow_direction=0;
57 _mp_last_reported_dist=0;
58 _mp_last_reported_v=0;
60 _mp_open_last_state=false;
61 _timeLagCorrectedDist=0;
63 _node->tie("tow/length",SGRawValuePointer<float>(&_towLength));
64 _node->tie("tow/elastic-constant",SGRawValuePointer<float>(&_towElasticConstant));
65 _node->tie("tow/weight-per-m-kg-m",SGRawValuePointer<float>(&_towWeightPerM));
66 _node->tie("tow/brake-force",SGRawValuePointer<float>(&_towBrakeForce));
67 _node->tie("winch/max-speed-m-s",SGRawValuePointer<float>(&_winchMaxSpeed));
68 _node->tie("winch/rel-speed",SGRawValuePointer<float>(&_winchRelSpeed));
69 _node->tie("winch/initial-tow-length-m",SGRawValuePointer<float>(&_winchInitialTowLength));
70 _node->tie("winch/min-tow-length-m",SGRawValuePointer<float>(&_winchMinTowLength));
71 _node->tie("winch/max-tow-length-m",SGRawValuePointer<float>(&_winchMaxTowLength));
72 _node->tie("winch/global-pos-x",SGRawValuePointer<double>(&_winchPos[0]));
73 _node->tie("winch/global-pos-y",SGRawValuePointer<double>(&_winchPos[1]));
74 _node->tie("winch/global-pos-z",SGRawValuePointer<double>(&_winchPos[2]));
75 _node->tie("winch/max-power",SGRawValuePointer<float>(&_winchPower));
76 _node->tie("winch/max-force",SGRawValuePointer<float>(&_winchMaxForce));
77 _node->tie("winch/actual-force",SGRawValuePointer<float>(&_winchActualForce));
78 _node->tie("tow/end-force-x",SGRawValuePointer<float>(&_reportTowEndForce[0]));
79 _node->tie("tow/end-force-y",SGRawValuePointer<float>(&_reportTowEndForce[1]));
80 _node->tie("tow/end-force-z",SGRawValuePointer<float>(&_reportTowEndForce[2]));
81 _node->tie("force",SGRawValuePointer<float>(&_forceMagnitude));
82 _node->tie("open",SGRawValuePointer<bool>(&_open));
83 _node->tie("force-is-calculated-by-other",SGRawValuePointer<bool>(&_forceIsCalculatedByMaster));
84 _node->tie("local-pos-x",SGRawValuePointer<float>(&_pos[0]));
85 _node->tie("local-pos-y",SGRawValuePointer<float>(&_pos[1]));
86 _node->tie("local-pos-z",SGRawValuePointer<float>(&_pos[2]));
87 _node->tie("tow/dist",SGRawValuePointer<float>(&_dist));
88 _node->tie("tow/dist-time-lag-corrected",SGRawValuePointer<float>(&_timeLagCorrectedDist));
89 _node->tie("tow/connected-to-property-node",SGRawValuePointer<bool>(&_towEndIsConnectedToProperty));
90 _node->tie("tow/connected-to-mp-node",SGRawValuePointer<bool>(&_nodeIsMultiplayer));
91 _node->tie("tow/connected-to-ai-node",SGRawValuePointer<bool>(&_nodeIsAiAircraft));
92 _node->tie("tow/connected-to-ai-or-mp-id",SGRawValuePointer<int>(&_nodeID));
93 _node->tie("debug/hitch-height-above-ground",SGRawValuePointer<float>(&_height_above_ground));
94 _node->tie("debug/tow-end-height-above-ground",SGRawValuePointer<float>(&_winch_height_above_ground));
95 _node->tie("debug/tow-rel-lo-pos",SGRawValuePointer<float>(&_loPosFrac));
96 _node->tie("debug/tow-lowest-pos-height",SGRawValuePointer<float>(&_lowest_tow_height));
97 _node->tie("is-slave",SGRawValuePointer<bool>(&_isSlave));
98 _node->tie("speed-in-tow-direction",SGRawValuePointer<float>(&_speed_in_tow_direction));
99 _node->tie("mp-auto-connect-period",SGRawValuePointer<float>(&_mpAutoConnectPeriod));
100 _node->tie("mp-time-lag",SGRawValuePointer<float>(&_mp_time_lag));
101 _node->setStringValue("tow/node","");
102 _node->setStringValue("tow/connected-to-ai-or-mp-callsign");
103 _node->setBoolValue("broken",false);
108 _node->untie("tow/length");
109 _node->untie("tow/elastic-constant");
110 _node->untie("tow/weight-per-m-kg-m");
111 _node->untie("tow/brake-force");
112 _node->untie("winch/max-speed-m-s");
113 _node->untie("winch/rel-speed");
114 _node->untie("winch/initial-tow-length-m");
115 _node->untie("winch/min-tow-length-m");
116 _node->untie("winch/max-tow-length-m");
117 _node->untie("winch/global-pos-x");
118 _node->untie("winch/global-pos-y");
119 _node->untie("winch/global-pos-z");
120 _node->untie("winch/max-power");
121 _node->untie("winch/max-force");
122 _node->untie("winch/actual-force");
123 _node->untie("tow/end-force-x");
124 _node->untie("tow/end-force-y");
125 _node->untie("tow/end-force-z");
126 _node->untie("force");
127 _node->untie("open");
128 _node->untie("force-is-calculated-by-other");
129 _node->untie("local-pos-x");
130 _node->untie("local-pos-y");
131 _node->untie("local-pos-z");
132 _node->untie("tow/dist");
133 _node->untie("tow/dist-time-lag-corrected");
134 _node->untie("tow/connected-to-property-node");
135 _node->untie("tow/connected-to-mp-node");
136 _node->untie("tow/connected-to-ai-node");
137 _node->untie("tow/connected-to-ai-or-mp-id");
138 _node->untie("debug/hitch-height-above-ground");
139 _node->untie("debug/tow-end-height-above-ground");
140 _node->untie("debug/tow-rel-lo-pos");
141 _node->untie("debug/tow-lowest-pos-height");
142 _node->untie("is-slave");
143 _node->untie("speed-in-tow-direction");
144 _node->untie("mp-auto-connect-period");
145 _node->untie("mp-time-lag");
150 void Hitch::setPosition(float* position)
153 for(i=0; i<3; i++) _pos[i] = position[i];
156 void Hitch::setTowLength(float length)
161 void Hitch::setOpen(bool isOpen)
163 //test if we already processed this before
164 //without this test a binded property could
165 //try to close the Hitch every run
166 //it will close, if we are near the end
167 //e.g. if we are flying over the parked
169 if (isOpen==_last_wish)
175 void Hitch::setTowElasticConstant(float sc)
177 _towElasticConstant=sc;
180 void Hitch::setTowBreakForce(float bf)
185 void Hitch::setWinchMaxForce(float f)
190 void Hitch::setTowWeightPerM(float rw)
195 void Hitch::setWinchMaxSpeed(float mws)
200 void Hitch::setWinchRelSpeed(float rws)
205 void Hitch::setWinchPosition(double *winchPosition)//in global coordinates!
207 for (int i=0; i<3;i++)
208 _winchPos[i]=winchPosition[i];
211 void Hitch::setMpAutoConnectPeriod(float dt)
213 _mpAutoConnectPeriod=dt;
216 void Hitch::setForceIsCalculatedByOther(bool b)
218 _forceIsCalculatedByMaster=b;
221 const char *Hitch::getConnectedPropertyNode() const
224 return _towEndNode->getDisplayName();
229 void Hitch::setConnectedPropertyNode(const char *nodename)
231 _towEndNode=fgGetNode(nodename,false);
234 void Hitch::setWinchPositionAuto(bool doit)
236 static bool lastState=false;
248 // The ground plane transformed to the local frame.
250 _state->planeGlobalToLocal(_global_ground, ground);
253 //find a normalized vector pointing forward parallel to the ground
257 Math::cross3(ground,help,help);
258 //multiplay by initial tow length;
259 //reduced by 1m to be able to close the
260 //hitch either if the glider slips backwards a bit
261 Math::mul3((_winchInitialTowLength-1.),help,help);
262 //add to the actual pos
263 Math::add3(_pos,help,lWinchPos);
264 //put it onto the ground plane
265 Math::mul3(ground[3],ground,help);
266 Math::add3(lWinchPos,help,lWinchPos);
268 _state->posLocalToGlobal(lWinchPos,_winchPos);
269 _towLength=_winchInitialTowLength;
270 fgSetString("/sim/messages/pilot", "Connected to winch!");
273 _node->setBoolValue("broken",false);
275 //set the dist value (if not, the hitch would open in the next calcforce run
277 //Math::sub3(lWinchPos,_pos,delta);
278 //_dist=Math::mag3(delta);
279 _dist=Math::mag3(lWinchPos); //use the aircraft center as reference for distance calculation
280 //this position is transferred to the MP-Aircraft.
281 //With this trick, both player in aerotow get the same length
285 void Hitch::findBestAIObject(bool doit,bool running_as_autoconnect)
287 static bool lastState=false;
290 if (!running_as_autoconnect)
292 //if this function is binded to an input, it will be called every frame as long as the key is pressed.
293 //therefore wait for a key-release before running it again.
304 _state->posLocalToGlobal(_pos,gpos);
305 double bestdist=_towLength*_towLength;//squared!
306 _towEndIsConnectedToProperty=false;
307 SGPropertyNode * ainode = fgGetNode("/ai/models",false);
309 char myCallsign[256]="***********";
310 if (running_as_autoconnect)
313 SGPropertyNode *cs=fgGetNode("/sim/multiplay/callsign",false);
316 strncpy(myCallsign,cs->getStringValue(),256);
319 //reset tow length for search radius. Lentgh will be later copied from master
320 _towLength=_winchInitialTowLength;
323 vector <SGPropertyNode_ptr> nodes;//<SGPropertyNode_ptr>
324 for (int i=0;i<ainode->nChildren();i++)
326 SGPropertyNode * n=ainode->getChild(i);
327 _nodeIsMultiplayer = strncmp("multiplayer",n->getName(),11)==0;
328 _nodeIsAiAircraft = strncmp("aircraft",n->getName(),8)==0;
329 if (!(_nodeIsMultiplayer || _nodeIsAiAircraft))
331 if (running_as_autoconnect)
333 if (!_nodeIsMultiplayer)
335 if(n->getBoolValue("sim/hitches/aerotow/open",true)) continue;
336 if(strncmp(myCallsign,n->getStringValue("sim/hitches/aerotow/tow/connected-to-ai-or-mp-callsign"),255)!=0)
340 pos[0]=n->getDoubleValue("position/global-x",0);
341 pos[1]=n->getDoubleValue("position/global-y",0);
342 pos[2]=n->getDoubleValue("position/global-z",0);
344 for (int j=0;j<3;j++)
345 dist+=(pos[j]-gpos[j])*(pos[j]-gpos[j]);
350 _towEndIsConnectedToProperty=true;
351 //_node->setStringValue("tow/node",n->getPath());
352 _node->setStringValue("tow/node",n->getDisplayName());
353 _nodeID=n->getIntValue("id",0);
354 _node->setStringValue("tow/connected-to-ai-or-mp-callsign",n->getStringValue("callsign"));
361 if (!running_as_autoconnect)
363 std::stringstream message;
364 message<<_node->getStringValue("tow/connected-to-ai-or-mp-callsign")
365 <<", I am on your hook, distance "<<Math::sqrt(bestdist)<<" meter.";
366 fgSetString("/sim/messages/pilot", message.str().c_str());
370 std::stringstream message;
371 message<<_node->getStringValue("tow/connected-to-ai-or-mp-callsign")
372 <<": I am on your hook, distance "<<Math::sqrt(bestdist)<<" meter.";
373 fgSetString("/sim/messages/ai-plane", message.str().c_str());
375 if (running_as_autoconnect)
377 //set the dist value to some value below the tow lentgh (if not, the hitch
378 //would open in the next calc force run
379 _dist=_towLength*0.5;
380 _mp_open_last_state=true;
383 if (!running_as_autoconnect)
385 fgSetString("/sim/messages/atc", "Sorry, no aircraft for aerotow!");
389 void Hitch::setWinchInitialTowLength(float length)
391 _winchInitialTowLength=length;
394 void Hitch::setWinchPower(float power)
399 void Hitch::setWinchMaxTowLength(float length)
401 _winchMaxTowLength=length;
404 void Hitch::setWinchMinTowLength(float length)
406 _winchMinTowLength=length;
409 void Hitch::setGlobalGround(double *global_ground, float *global_vel)
412 for(i=0; i<4; i++) _global_ground[i] = global_ground[i];
413 for(i=0; i<3; i++) _global_vel[i] = global_vel[i];
416 void Hitch::getPosition(float* out)
419 for(i=0; i<3; i++) out[i] = _pos[i];
422 float Hitch::getTowLength(void)
427 void Hitch::calcForce(Ground *g_cb, RigidBody* body, State* s)
429 float lWinchPos[3],delta[3],deltaN[3];
431 s->posGlobalToLocal(_winchPos,lWinchPos);
432 Math::sub3(lWinchPos,_pos,delta);
433 //_dist=Math::mag3(delta);
434 _dist=Math::mag3(lWinchPos); //use the aircraft center as reference for distance calculation
435 //this position is transferred to the MP-Aircraft.
436 //With this trick, both player in aerotow get the same length
437 Math::unit3(delta,deltaN);
439 s->velGlobalToLocal(s->v,lvel);
440 _speed_in_tow_direction=Math::dot3(lvel,deltaN);
441 if (_towEndIsConnectedToProperty && _nodeIsMultiplayer)
443 float mp_delta_dist_due_to_time_lag=0.5*_mp_time_lag*(-_mp_v+_speed_in_tow_direction);
444 _timeLagCorrectedDist=_dist+mp_delta_dist_due_to_time_lag;
445 if(_forceIsCalculatedByMaster && !_open)
447 s->velGlobalToLocal(_mp_force,_force);
452 _timeLagCorrectedDist=_dist;
455 _force[0]=_force[1]=_force[2]=0;
460 _forceMagnitude=(_dist-_towLength)/_towLength*_towElasticConstant;
462 _forceMagnitude=2*_towBrakeForce;
465 if(_forceMagnitude>=_towBrakeForce)
469 _node->setBoolValue("broken",true);
470 _force[0]=_force[1]=_force[2]=0;
471 _towEndForce[0]=_towEndForce[1]=_towEndForce[2]=0;
472 _reportTowEndForce[0]=_reportTowEndForce[1]=_reportTowEndForce[2]=0;
475 Math::mul3(_forceMagnitude,deltaN,_force);
476 Math::mul3(-1.,_force,_towEndForce);
477 _winchActualForce=_forceMagnitude; //missing: gravity on this end and friction
478 //Add the gravitiy of the rope.
479 //calculate some numbers:
480 float grav_force=_towWeightPerM*_towLength*9.81;
481 //the length of the gravity-expanded row:
482 float leng=_towLength+grav_force*_towLength/_towElasticConstant;
483 // The ground plane transformed to the local frame.
485 s->planeGlobalToLocal(_global_ground, ground);
487 // The velocity of the contact patch transformed to local coordinates.
489 //s->velGlobalToLocal(_global_vel, glvel);
491 _height_above_ground = ground[3] - Math::dot3(_pos, ground);
493 //the same for the winch-pos (the pos of the tow end)
494 _winch_height_above_ground = ground[3] - Math::dot3(lWinchPos, ground);
496 //the frac of the grav force acting on _pos:
497 float grav_frac=0.5*(1+(_height_above_ground-_winch_height_above_ground)/leng);
498 grav_frac=Math::clamp(grav_frac,0,1);
499 float grav_frac_tow_end=1-grav_frac;
500 //reduce grav_frac, if the tow has ground contact.
501 if (_height_above_ground<leng) //if not, the tow can not be on ground
503 float fa[3],fb[3],fg[3];
504 //the grav force an the hitch position:
505 Math::mul3(-grav_frac*grav_force,ground,fg);
506 //the total force on hitch postion:
507 Math::add3(fg,_force,fa);
508 //the grav force an the tow end position:
509 Math::mul3(-(1-grav_frac)*grav_force,ground,fg);
510 //the total force on tow end postion:
511 //note: sub: _force on tow-end is negative of force on hitch postion
512 Math::sub3(fg,_force,fb);
513 float fa_=Math::mag3(fa);
514 float fb_=Math::mag3(fb);
515 float stretchedTowLen;
516 stretchedTowLen=_towLength*(1.+(fa_+fb_)/(2*_towElasticConstant));
517 //the relative position of the lowest postion of the tow:
519 _loPosFrac=fa_/(fa_+fb_);
522 //dist to tow-end parallel to ground
525 //Math::cross3(delta,ground,help);//as long as we calculate the dist without _pos, od it with lWinchpos, the dist to our center....
526 Math::cross3(lWinchPos,ground,help);
527 ground_dist=Math::mag3(help);
528 //height of lowest tow pos (relative to _pos)
529 _lowest_tow_height=_loPosFrac*Math::sqrt(Math::abs(stretchedTowLen*stretchedTowLen-ground_dist*ground_dist));
530 if (_height_above_ground<_lowest_tow_height)
532 if (_height_above_ground>1e-3)
533 grav_frac*=_height_above_ground/_lowest_tow_height;
537 if (_winch_height_above_ground<(_lowest_tow_height-_height_above_ground+_winch_height_above_ground))
539 if (_winch_height_above_ground>1e-3)
540 grav_frac_tow_end*=_winch_height_above_ground/
541 (_lowest_tow_height-_height_above_ground+_winch_height_above_ground);
546 else _lowest_tow_height=_loPosFrac=-1; //for debug output
547 float grav_force_v[3];
548 Math::mul3(grav_frac*grav_force,ground,grav_force_v);
549 Math::add3(grav_force_v,_force,_force);
550 _forceMagnitude=Math::mag3(_force);
551 //the same for the tow end:
552 Math::mul3(grav_frac_tow_end*grav_force,ground,grav_force_v);
553 Math::add3(grav_force_v,_towEndForce,_towEndForce);
554 s->velLocalToGlobal(_towEndForce,_towEndForce);
556 if(_forceMagnitude>=_towBrakeForce)
560 _node->setBoolValue("broken",true);
561 _force[0]=_force[1]=_force[2]=0;
562 _towEndForce[0]=_towEndForce[1]=_towEndForce[2]=0;
568 // Computed values: total force
569 void Hitch::getForce(float* force, float* off)
571 Math::set3(_force, force);
572 Math::set3(_pos, off);
575 void Hitch::integrate (float dt)
577 //check if hitch has opened or closed, if yes: message
578 if (_open !=_oldOpen)
582 if (_dist>_towLength*1.00001)
584 std::stringstream message;
585 message<<"Could not lock hitch (tow length is insufficient) on hitch '"
586 <<_node->getPath()<<"' !";
587 fgSetString("/sim/messages/pilot", message.str().c_str());
591 _node->setBoolValue("broken",false);
593 std::stringstream message;
594 if (_node->getBoolValue("broken",false)&&_open)
595 message<<"Oh no, the tow is broken";
597 message<<(_open?"Opened hitch '":"Locked hitch '")<<_node->getPath()<<"'!";
598 fgSetString("/sim/messages/pilot", message.str().c_str());
602 //check, if tow end should be searched in all MP-aircrafts
603 if(_open && _mpAutoConnectPeriod)
606 _timeToNextAutoConnectTry-=dt;
607 if ((_timeToNextAutoConnectTry>_mpAutoConnectPeriod) || (_timeToNextAutoConnectTry<0))
609 _timeToNextAutoConnectTry=_mpAutoConnectPeriod;
610 //search for MP-Aircraft, which is towed with us
611 findBestAIObject(true,true);
614 //check, if tow end can be modified by property, if yes: update
615 if(_towEndIsConnectedToProperty)
619 //_towEndNode=fgGetNode(_node->getStringValue("tow/node"), false);
621 strncpy(towNode,_node->getStringValue("tow/node"),256);
623 _towEndNode=fgGetNode("ai/models")->getNode(towNode, false);
624 //AI and multiplayer objects seem to change node?
625 //Check if we have the right one by callsign
626 if (_nodeIsMultiplayer || _nodeIsAiAircraft)
628 char MPcallsign[256]="";
630 MPc=_node->getStringValue("tow/connected-to-ai-or-mp-callsign");
633 strncpy(MPcallsign,MPc,256);
636 if (((_towEndNode)&&(strncmp(_towEndNode->getStringValue("callsign"),MPcallsign,255)!=0))||!_towEndNode)
638 _timeToNextReConnectTry-=dt;
639 if((_timeToNextReConnectTry<0)||(_timeToNextReConnectTry>10))
641 _timeToNextReConnectTry=10;
642 SGPropertyNode * ainode = fgGetNode("/ai/models",false);
645 for (int i=0;i<ainode->nChildren();i++)
647 SGPropertyNode * n=ainode->getChild(i);
648 if(_nodeIsMultiplayer?strncmp("multiplayer",n->getName(),11)==0:strncmp("aircraft",n->getName(),8))
649 if (strcmp(n->getStringValue("callsign"),MPcallsign)==0)//found
652 //_node->setStringValue("tow/node",n->getPath());
653 _node->setStringValue("tow/node",n->getDisplayName());
662 _winchPos[0]=_towEndNode->getDoubleValue("position/global-x",_winchPos[0]);
663 _winchPos[1]=_towEndNode->getDoubleValue("position/global-y",_winchPos[1]);
664 _winchPos[2]=_towEndNode->getDoubleValue("position/global-z",_winchPos[2]);
665 _mp_lpos[0]=_towEndNode->getFloatValue("sim/hitches/aerotow/local-pos-x",0);
666 _mp_lpos[1]=_towEndNode->getFloatValue("sim/hitches/aerotow/local-pos-y",0);
667 _mp_lpos[2]=_towEndNode->getFloatValue("sim/hitches/aerotow/local-pos-z",0);
668 _mp_dist=_towEndNode->getFloatValue("sim/hitches/aerotow/tow/dist");
669 _mp_v=_towEndNode->getFloatValue("sim/hitches/aerotow/speed-in-tow-direction");
670 _mp_force[0]=_towEndNode->getFloatValue("sim/hitches/aerotow/tow/end-force-x",0);
671 _mp_force[1]=_towEndNode->getFloatValue("sim/hitches/aerotow/tow/end-force-y",0);
672 _mp_force[2]=_towEndNode->getFloatValue("sim/hitches/aerotow/tow/end-force-z",0);
676 #define gf(a,b) a=_towEndNode->getFloatValue(b,a)
677 #define gb(a,b) a=_towEndNode->getBoolValue(b,a)
678 gf(_towLength,"sim/hitches/aerotow/tow/length");
679 gf(_towElasticConstant,"sim/hitches/aerotow/tow/elastic-constant");
680 gf(_towWeightPerM,"sim/hitches/aerotow/tow/weight-per-m-kg-m");
681 gf(_towBrakeForce,"sim/hitches/aerotow/brake-force");
682 gb(_open,"sim/hitches/aerotow/open");
683 gb(_mp_is_slave,"sim/hitches/aerotow/is-slave");
686 if (_mp_is_slave) _isSlave=false; //someone should be master
690 //check if other has opened hitch, but is neccessary, that it was closed before
691 bool mp_open=_towEndNode->getBoolValue("sim/hitches/aerotow/open",_mp_open_last_state);
692 if (mp_open != _mp_open_last_state) //state has changed
694 _mp_open_last_state=mp_open; //store that value
700 std::stringstream message;
701 message<<_node->getStringValue("tow/connected-to-ai-or-mp-callsign")
702 <<": I have released the tow!";
703 fgSetString("/sim/messages/ai-plane", message.str().c_str());
708 //try to calculate the time lag
709 if ((_mp_last_reported_dist!=_mp_dist)||(_mp_last_reported_v!=_mp_v)) //new data;
711 _mp_last_reported_dist=_mp_dist;
712 _mp_last_reported_v=_mp_v;
713 float total_v=-_mp_v+_speed_in_tow_direction;//mp has opposite tow direction
714 float abs_v=Math::abs(total_v);
717 float actual_time_lag_guess=(_mp_dist-_dist)/total_v;
718 //check, if it sounds ok
719 if((actual_time_lag_guess>0)&&(actual_time_lag_guess<5))
721 float frac=abs_v*0.01;
722 if (frac>0.05) frac=0.05;
723 // if we are slow, the guess of the lag can be rather wrong. as faster we are
724 // the better the guess. Therefore frac is proportiona to the speed. Clamp it
726 _mp_time_lag=(1-frac)*_mp_time_lag+frac*actual_time_lag_guess;
733 //set the _reported_tow_end_force (smoothed)
734 //smooth it a bit and store it
735 float sc=10.*dt; //100ms
737 Math::mul3(sc,_towEndForce,tmp);
738 Math::mul3(1.-sc,_reportTowEndForce,_reportTowEndForce);
739 Math::add3(tmp,_reportTowEndForce,_reportTowEndForce);
742 if (_winchRelSpeed==0) return;
743 float factor=1,offset=0;
744 if (_winchActualForce>_winchMaxForce)
745 offset=-(_winchActualForce-_winchMaxForce)/_winchMaxForce*20;
746 if (_winchRelSpeed>0.01) // to avoit div by zero
748 float maxForcePowerLimit=_winchPower/(_winchRelSpeed*_winchMaxSpeed);
749 if (_winchActualForce>maxForcePowerLimit)
750 factor-=(_winchActualForce-maxForcePowerLimit)/maxForcePowerLimit;
752 _towLength-=dt*(factor*_winchRelSpeed+offset)*_winchMaxSpeed;
753 if (_towLength<=_winchMinTowLength)
755 if (_winchRelSpeed<0)
757 _towLength=_winchMinTowLength;
760 if (_towLength>=_winchMaxTowLength)
762 if (_winchRelSpeed<0)
764 _towLength=_winchMaxTowLength;
772 }; // namespace yasim