1 // FGAIMultiplayer - FGAIBase-derived class creates an AI multiplayer aircraft
3 // Based on FGAIAircraft
4 // Written by David Culp, started October 2003.
5 // Also by Gregor Richards, started December 2005.
7 // Copyright (C) 2003 David P. Culp - davidculp2@comcast.net
8 // Copyright (C) 2005 Gregor Richards
10 // This program is free software; you can redistribute it and/or
11 // modify it under the terms of the GNU General Public License as
12 // published by the Free Software Foundation; either version 2 of the
13 // License, or (at your option) any later version.
15 // This program is distributed in the hope that it will be useful, but
16 // WITHOUT ANY WARRANTY; without even the implied warranty of
17 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
18 // General Public License for more details.
20 // You should have received a copy of the GNU General Public License
21 // along with this program; if not, write to the Free Software
22 // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
31 #include "AIMultiplayer.hxx"
35 // #define SG_DEBUG SG_ALERT
37 FGAIMultiplayer::FGAIMultiplayer() :
38 FGAIBase(otMultiplayer, fgGetBool("/sim/multiplay/hot", false))
42 mTimeOffsetSet = false;
43 mAllowExtrapolation = true;
44 mLagAdjustSystemSpeed = 10;
52 FGAIMultiplayer::~FGAIMultiplayer() {
55 bool FGAIMultiplayer::init(bool search_in_AI_path) {
56 props->setStringValue("sim/model/path", model_path.c_str());
57 //refuel_node = fgGetNode("systems/refuel/contact", true);
58 isTanker = false; // do this until this property is
59 // passed over the net
61 const string& str1 = _getCallsign();
62 const string str2 = "MOBIL";
64 string::size_type loc1= str1.find( str2, 0 );
65 if ( (loc1 != string::npos && str2 != "") ){
66 // cout << " string found " << str2 << " in " << str1 << endl;
68 // cout << "isTanker " << isTanker << " " << mCallSign <<endl;
72 bool result = FGAIBase::init(search_in_AI_path);
73 // propagate installation state (used by MP pilot list)
74 props->setBoolValue("model-installed", _installed);
78 void FGAIMultiplayer::bind() {
81 tie("refuel/contact", SGRawValuePointer<bool>(&contact));
82 tie("tanker", SGRawValuePointer<bool>(&isTanker));
84 tie("controls/invisible",
85 SGRawValuePointer<bool>(&invisible));
86 _uBodyNode = props->getNode("velocities/uBody-fps", true);
87 _vBodyNode = props->getNode("velocities/vBody-fps", true);
88 _wBodyNode = props->getNode("velocities/wBody-fps", true);
90 #define AIMPROProp(type, name) \
91 SGRawValueMethods<FGAIMultiplayer, type>(*this, &FGAIMultiplayer::get##name)
93 #define AIMPRWProp(type, name) \
94 SGRawValueMethods<FGAIMultiplayer, type>(*this, \
95 &FGAIMultiplayer::get##name, &FGAIMultiplayer::set##name)
97 //tie("callsign", AIMPROProp(const char *, CallSign));
99 tie("controls/allow-extrapolation",
100 AIMPRWProp(bool, AllowExtrapolation));
101 tie("controls/lag-adjust-system-speed",
102 AIMPRWProp(double, LagAdjustSystemSpeed));
103 tie("controls/player-lag",
104 AIMPRWProp(double, playerLag));
105 tie("controls/compensate-lag",
106 AIMPRWProp(int, compensateLag));
112 void FGAIMultiplayer::update(double dt)
114 using namespace simgear;
119 FGAIBase::update(dt);
121 // Check if we already got data
122 if (mMotionInfo.empty())
125 // The current simulation time we need to update for,
126 // note that the simulation time is updated before calling all the
127 // update methods. Thus it contains the time intervals *end* time
128 double curtime = globals->get_sim_time_sec();
130 // Get the last available time
131 MotionInfo::reverse_iterator it = mMotionInfo.rbegin();
132 double curentPkgTime = it->second.time;
134 // Dynamically optimize the time offset between the feeder and the client
135 // Well, 'dynamically' means that the dynamic of that update must be very
136 // slow. You would otherwise notice huge jumps in the multiplayer models.
137 // The reason is that we want to avoid huge extrapolation times since
138 // extrapolation is highly error prone. For that we need something
139 // approaching the average latency of the packets. This first order lag
140 // component will provide this. We just take the error of the currently
141 // requested time to the most recent available packet. This is the
142 // target we want to reach in average.
143 double lag = it->second.lag;
144 if (!mTimeOffsetSet) {
145 mTimeOffsetSet = true;
146 mTimeOffset = curentPkgTime - curtime - lag;
150 //spectator mode, more late to be in the interpolation zone
151 if (compensateLag == 3) { offset = curentPkgTime -curtime -lag + playerLag;
154 } else if (compensateLag == 1) { offset = curentPkgTime - curtime - lag;
156 // using the prediction mode to display the mpaircraft in the futur/past with given playerlag value
157 //currently compensatelag = 2
158 } else { offset = curentPkgTime - curtime + 0.48*lag + playerLag;
160 if ((!mAllowExtrapolation && offset + lag < mTimeOffset)
161 || (offset - 10 > mTimeOffset)) {
162 mTimeOffset = offset;
163 SG_LOG(SG_AI, SG_DEBUG, "Resetting time offset adjust system to "
164 "avoid extrapolation: time offset = " << mTimeOffset);
166 // the error of the offset, respectively the negative error to avoid
168 double err = offset - mTimeOffset;
169 // limit errors leading to shorter lag values somehow, that is late
170 // arriving packets will pessimize the overall lag much more than
171 // early packets will shorten the overall lag
174 //trying to slow the rudderlag phenomenon thus using more the prediction system
175 //if we are off by less than 1.5s, do a little correction, and bigger step above 1.5s
176 if (fabs(err) < 1.5) {
178 sysSpeed = mLagAdjustSystemSpeed*err*0.01;
180 sysSpeed = SGMiscd::min(0.5*err*err, 0.05);
185 // Ok, we have some very late packets and nothing newer increase the
186 // lag by the given speedadjust
187 sysSpeed = mLagAdjustSystemSpeed*err;
189 // We have a too pessimistic display delay shorten that a small bit
190 sysSpeed = SGMiscd::min(0.1*err*err, 0.5);
195 // simple euler integration for that first order system including some
196 // overshooting guard to prevent to aggressive system speeds
197 // (stiff systems) to explode the systems state
198 double systemIncrement = dt*sysSpeed;
199 if (fabs(err) < fabs(systemIncrement))
200 systemIncrement = err;
201 mTimeOffset += systemIncrement;
203 SG_LOG(SG_AI, SG_DEBUG, "Offset adjust system: time offset = "
204 << mTimeOffset << ", expected longitudinal position error due to "
205 " current adjustment of the offset: "
206 << fabs(norm(it->second.linearVel)*systemIncrement));
211 // Compute the time in the feeders time scale which fits the current time
213 double tInterp = curtime + mTimeOffset;
219 if (tInterp <= curentPkgTime) {
220 // Ok, we need a time prevous to the last available packet,
223 // Find the first packet before the target time
224 MotionInfo::iterator nextIt = mMotionInfo.upper_bound(tInterp);
225 if (nextIt == mMotionInfo.begin()) {
226 SG_LOG(SG_AI, SG_DEBUG, "Taking oldest packet!");
227 // We have no packet before the target time, just use the first one
228 MotionInfo::iterator firstIt = mMotionInfo.begin();
229 ecPos = firstIt->second.position;
230 ecOrient = firstIt->second.orientation;
231 ecLinearVel = firstIt->second.linearVel;
232 speed = norm(ecLinearVel) * SG_METER_TO_NM * 3600.0;
234 std::vector<FGPropertyData*>::const_iterator firstPropIt;
235 std::vector<FGPropertyData*>::const_iterator firstPropItEnd;
236 firstPropIt = firstIt->second.properties.begin();
237 firstPropItEnd = firstIt->second.properties.end();
238 while (firstPropIt != firstPropItEnd) {
239 //cout << " Setting property..." << (*firstPropIt)->id;
240 PropertyMap::iterator pIt = mPropertyMap.find((*firstPropIt)->id);
241 if (pIt != mPropertyMap.end())
243 //cout << "Found " << pIt->second->getPath() << ":";
244 switch ((*firstPropIt)->type) {
248 pIt->second->setIntValue((*firstPropIt)->int_value);
249 //cout << "Int: " << (*firstPropIt)->int_value << "\n";
253 pIt->second->setFloatValue((*firstPropIt)->float_value);
254 //cout << "Flo: " << (*firstPropIt)->float_value << "\n";
257 case props::UNSPECIFIED:
258 pIt->second->setStringValue((*firstPropIt)->string_value);
259 //cout << "Str: " << (*firstPropIt)->string_value << "\n";
262 // FIXME - currently defaults to float values
263 pIt->second->setFloatValue((*firstPropIt)->float_value);
264 //cout << "Unknown: " << (*firstPropIt)->float_value << "\n";
270 SG_LOG(SG_AI, SG_DEBUG, "Unable to find property: " << (*firstPropIt)->id << "\n");
276 // Ok, we have really found something where our target time is in between
277 // do interpolation here
278 MotionInfo::iterator prevIt = nextIt;
281 // Interpolation coefficient is between 0 and 1
282 double intervalStart = prevIt->second.time;
283 double intervalEnd = nextIt->second.time;
284 double intervalLen = intervalEnd - intervalStart;
285 double tau = (tInterp - intervalStart)/intervalLen;
287 SG_LOG(SG_AI, SG_DEBUG, "Multiplayer vehicle interpolation: ["
288 << intervalStart << ", " << intervalEnd << "], intervalLen = "
289 << intervalLen << ", interpolation parameter = " << tau);
291 // Here we do just linear interpolation on the position
292 ecPos = ((1-tau)*prevIt->second.position + tau*nextIt->second.position);
293 ecOrient = interpolate((float)tau, prevIt->second.orientation,
294 nextIt->second.orientation);
295 ecLinearVel = ((1-tau)*prevIt->second.linearVel + tau*nextIt->second.linearVel);
296 speed = norm(ecLinearVel) * SG_METER_TO_NM * 3600.0;
298 if (prevIt->second.properties.size()
299 == nextIt->second.properties.size()) {
300 std::vector<FGPropertyData*>::const_iterator prevPropIt;
301 std::vector<FGPropertyData*>::const_iterator prevPropItEnd;
302 std::vector<FGPropertyData*>::const_iterator nextPropIt;
303 std::vector<FGPropertyData*>::const_iterator nextPropItEnd;
304 prevPropIt = prevIt->second.properties.begin();
305 prevPropItEnd = prevIt->second.properties.end();
306 nextPropIt = nextIt->second.properties.begin();
307 nextPropItEnd = nextIt->second.properties.end();
308 while (prevPropIt != prevPropItEnd) {
309 PropertyMap::iterator pIt = mPropertyMap.find((*prevPropIt)->id);
310 //cout << " Setting property..." << (*prevPropIt)->id;
312 if (pIt != mPropertyMap.end())
314 //cout << "Found " << pIt->second->getPath() << ":";
318 switch ((*prevPropIt)->type) {
322 ival = (int) (0.5+(1-tau)*((double) (*prevPropIt)->int_value) +
323 tau*((double) (*nextPropIt)->int_value));
324 pIt->second->setIntValue(ival);
325 //cout << "Int: " << ival << "\n";
329 val = (1-tau)*(*prevPropIt)->float_value +
330 tau*(*nextPropIt)->float_value;
331 //cout << "Flo: " << val << "\n";
332 pIt->second->setFloatValue(val);
335 case props::UNSPECIFIED:
336 //cout << "Str: " << (*nextPropIt)->string_value << "\n";
337 pIt->second->setStringValue((*nextPropIt)->string_value);
340 // FIXME - currently defaults to float values
341 val = (1-tau)*(*prevPropIt)->float_value +
342 tau*(*nextPropIt)->float_value;
343 //cout << "Unk: " << val << "\n";
344 pIt->second->setFloatValue(val);
350 SG_LOG(SG_AI, SG_DEBUG, "Unable to find property: " << (*prevPropIt)->id << "\n");
358 // Now throw away too old data
359 if (prevIt != mMotionInfo.begin())
362 mMotionInfo.erase(mMotionInfo.begin(), prevIt);
366 // Ok, we need to predict the future, so, take the best data we can have
367 // and do some eom computation to guess that for now.
368 FGExternalMotionData& motionInfo = it->second;
370 // The time to predict, limit to 3 seconds
371 double t = tInterp - motionInfo.time;
372 t = SGMisc<double>::min(t, 3);
374 SG_LOG(SG_AI, SG_DEBUG, "Multiplayer vehicle extrapolation: "
375 "extrapolation time = " << t);
377 // using velocity and acceleration to guess a parabolic position...
378 ecPos = motionInfo.position;
379 ecOrient = motionInfo.orientation;
380 ecLinearVel = motionInfo.linearVel;
381 SGVec3d ecVel = toVec3d(ecOrient.backTransform(ecLinearVel));
382 SGVec3f angularVel = motionInfo.angularVel;
383 SGVec3d ecAcc = toVec3d(ecOrient.backTransform(motionInfo.linearAccel));
385 double normVel = norm(ecVel);
387 // not doing rotationnal prediction for small speed or rotation rate,
388 // to avoid agitated parked plane
389 if (( norm(angularVel) > 0.05 ) || ( normVel > 1.0 )) {
390 ecOrient += t*ecOrient.derivative(angularVel);
393 // not using acceleration for small speed, to have better parked planes
394 // note that anyway acceleration is not transmit yet by mp
395 if ( normVel > 1.0 ) {
396 ecPos += t*(ecVel + 0.5*t*ecAcc);
401 std::vector<FGPropertyData*>::const_iterator firstPropIt;
402 std::vector<FGPropertyData*>::const_iterator firstPropItEnd;
403 speed = norm(ecLinearVel) * SG_METER_TO_NM * 3600.0;
404 firstPropIt = it->second.properties.begin();
405 firstPropItEnd = it->second.properties.end();
406 while (firstPropIt != firstPropItEnd) {
407 PropertyMap::iterator pIt = mPropertyMap.find((*firstPropIt)->id);
408 //cout << " Setting property..." << (*firstPropIt)->id;
410 if (pIt != mPropertyMap.end())
412 switch ((*firstPropIt)->type) {
416 pIt->second->setIntValue((*firstPropIt)->int_value);
417 //cout << "Int: " << (*firstPropIt)->int_value << "\n";
421 pIt->second->setFloatValue((*firstPropIt)->float_value);
422 //cout << "Flo: " << (*firstPropIt)->float_value << "\n";
425 case props::UNSPECIFIED:
426 pIt->second->setStringValue((*firstPropIt)->string_value);
427 //cout << "Str: " << (*firstPropIt)->string_value << "\n";
430 // FIXME - currently defaults to float values
431 pIt->second->setFloatValue((*firstPropIt)->float_value);
432 //cout << "Unk: " << (*firstPropIt)->float_value << "\n";
438 SG_LOG(SG_AI, SG_DEBUG, "Unable to find property: " << (*firstPropIt)->id << "\n");
445 // extract the position
446 pos = SGGeod::fromCart(ecPos);
447 double recent_alt_ft = altitude_ft;
448 altitude_ft = pos.getElevationFt();
450 // expose a valid vertical speed
451 if (lastUpdateTime != 0)
453 double dT = curtime - lastUpdateTime;
457 // simple smoothing over 1 second
458 vs = (1.0-Weighting)*vs + Weighting * (altitude_ft - recent_alt_ft) / dT * 60;
460 lastUpdateTime = curtime;
462 // The quaternion rotating from the earth centered frame to the
463 // horizontal local frame
464 SGQuatf qEc2Hl = SGQuatf::fromLonLatRad((float)pos.getLongitudeRad(),
465 (float)pos.getLatitudeRad());
466 // The orientation wrt the horizontal local frame
467 SGQuatf hlOr = conj(qEc2Hl)*ecOrient;
468 float hDeg, pDeg, rDeg;
469 hlOr.getEulerDeg(hDeg, pDeg, rDeg);
474 // expose velocities/u,v,wbody-fps in the mp tree
475 _uBodyNode->setValue(ecLinearVel[0] * SG_METER_TO_FEET);
476 _vBodyNode->setValue(ecLinearVel[1] * SG_METER_TO_FEET);
477 _wBodyNode->setValue(ecLinearVel[2] * SG_METER_TO_FEET);
479 SG_LOG(SG_AI, SG_DEBUG, "Multiplayer position and orientation: "
480 << ecPos << ", " << hlOr);
482 //###########################//
483 // do calculations for radar //
484 //###########################//
485 double range_ft2 = UpdateRadar(manager);
487 //************************************//
489 //************************************//
493 //cout << "IS tanker ";
494 if ( (range_ft2 < 250.0 * 250.0) &&
497 // refuel_node->setBoolValue(true);
498 //cout << "in contact" << endl;
501 // refuel_node->setBoolValue(false);
502 //cout << "not in contact" << endl;
506 //cout << "NOT tanker " << endl;
514 FGAIMultiplayer::addMotionInfo(FGExternalMotionData& motionInfo,
517 mLastTimestamp = stamp;
519 if (!mMotionInfo.empty()) {
520 double diff = motionInfo.time - mMotionInfo.rbegin()->first;
522 // packet is very old -- MP has probably reset (incl. his timebase)
526 // drop packets arriving out of order
530 mMotionInfo[motionInfo.time] = motionInfo;
531 // We just copied the property (pointer) list - they are ours now. Clear the
532 // properties list in given/returned object, so former owner won't deallocate them.
533 motionInfo.properties.clear();
537 FGAIMultiplayer::setDoubleProperty(const std::string& prop, double val)
539 SGPropertyNode* pNode = props->getChild(prop.c_str(), true);
540 pNode->setDoubleValue(val);