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.
30 #include "AIMultiplayer.hxx"
34 // #define SG_DEBUG SG_ALERT
36 FGAIMultiplayer::FGAIMultiplayer() :
37 FGAIBase(otMultiplayer, false)
41 mTimeOffsetSet = false;
42 mAllowExtrapolation = true;
43 mLagAdjustSystemSpeed = 10;
49 FGAIMultiplayer::~FGAIMultiplayer() {
52 bool FGAIMultiplayer::init(bool search_in_AI_path) {
53 props->setStringValue("sim/model/path", model_path.c_str());
54 //refuel_node = fgGetNode("systems/refuel/contact", true);
55 isTanker = false; // do this until this property is
56 // passed over the net
58 const string& str1 = _getCallsign();
59 const string str2 = "MOBIL";
61 string::size_type loc1= str1.find( str2, 0 );
62 if ( (loc1 != string::npos && str2 != "") ){
63 // cout << " string found " << str2 << " in " << str1 << endl;
65 // cout << "isTanker " << isTanker << " " << mCallSign <<endl;
69 bool result = FGAIBase::init(search_in_AI_path);
70 // propagate installation state (used by MP pilot list)
71 props->setBoolValue("model-installed", _installed);
75 void FGAIMultiplayer::bind() {
78 tie("refuel/contact", SGRawValuePointer<bool>(&contact));
79 tie("tanker", SGRawValuePointer<bool>(&isTanker));
81 tie("controls/invisible",
82 SGRawValuePointer<bool>(&invisible));
84 #define AIMPROProp(type, name) \
85 SGRawValueMethods<FGAIMultiplayer, type>(*this, &FGAIMultiplayer::get##name)
87 #define AIMPRWProp(type, name) \
88 SGRawValueMethods<FGAIMultiplayer, type>(*this, \
89 &FGAIMultiplayer::get##name, &FGAIMultiplayer::set##name)
91 //tie("callsign", AIMPROProp(const char *, CallSign));
93 tie("controls/allow-extrapolation",
94 AIMPRWProp(bool, AllowExtrapolation));
95 tie("controls/lag-adjust-system-speed",
96 AIMPRWProp(double, LagAdjustSystemSpeed));
103 void FGAIMultiplayer::update(double dt)
105 using namespace simgear;
110 FGAIBase::update(dt);
112 // Check if we already got data
113 if (mMotionInfo.empty())
116 // The current simulation time we need to update for,
117 // note that the simulation time is updated before calling all the
118 // update methods. Thus it contains the time intervals *end* time
119 double curtime = globals->get_sim_time_sec();
121 // Get the last available time
122 MotionInfo::reverse_iterator it = mMotionInfo.rbegin();
123 double curentPkgTime = it->second.time;
125 // Dynamically optimize the time offset between the feeder and the client
126 // Well, 'dynamically' means that the dynamic of that update must be very
127 // slow. You would otherwise notice huge jumps in the multiplayer models.
128 // The reason is that we want to avoid huge extrapolation times since
129 // extrapolation is highly error prone. For that we need something
130 // approaching the average latency of the packets. This first order lag
131 // component will provide this. We just take the error of the currently
132 // requested time to the most recent available packet. This is the
133 // target we want to reach in average.
134 double lag = it->second.lag;
135 if (!mTimeOffsetSet) {
136 mTimeOffsetSet = true;
137 mTimeOffset = curentPkgTime - curtime - lag;
139 double offset = curentPkgTime - curtime - lag;
140 if ((!mAllowExtrapolation && offset + lag < mTimeOffset)
141 || (offset - 10 > mTimeOffset)) {
142 mTimeOffset = offset;
143 SG_LOG(SG_AI, SG_DEBUG, "Resetting time offset adjust system to "
144 "avoid extrapolation: time offset = " << mTimeOffset);
146 // the error of the offset, respectively the negative error to avoid
148 double err = offset - mTimeOffset;
149 // limit errors leading to shorter lag values somehow, that is late
150 // arriving packets will pessimize the overall lag much more than
151 // early packets will shorten the overall lag
154 // Ok, we have some very late packets and nothing newer increase the
155 // lag by the given speedadjust
156 sysSpeed = mLagAdjustSystemSpeed*err;
158 // We have a too pessimistic display delay shorten that a small bit
159 sysSpeed = SGMiscd::min(0.1*err*err, 0.5);
162 // simple euler integration for that first order system including some
163 // overshooting guard to prevent to aggressive system speeds
164 // (stiff systems) to explode the systems state
165 double systemIncrement = dt*sysSpeed;
166 if (fabs(err) < fabs(systemIncrement))
167 systemIncrement = err;
168 mTimeOffset += systemIncrement;
170 SG_LOG(SG_AI, SG_DEBUG, "Offset adjust system: time offset = "
171 << mTimeOffset << ", expected longitudinal position error due to "
172 " current adjustment of the offset: "
173 << fabs(norm(it->second.linearVel)*systemIncrement));
178 // Compute the time in the feeders time scale which fits the current time
180 double tInterp = curtime + mTimeOffset;
185 if (tInterp <= curentPkgTime) {
186 // Ok, we need a time prevous to the last available packet,
189 // Find the first packet before the target time
190 MotionInfo::iterator nextIt = mMotionInfo.upper_bound(tInterp);
191 if (nextIt == mMotionInfo.begin()) {
192 SG_LOG(SG_AI, SG_DEBUG, "Taking oldest packet!");
193 // We have no packet before the target time, just use the first one
194 MotionInfo::iterator firstIt = mMotionInfo.begin();
195 ecPos = firstIt->second.position;
196 ecOrient = firstIt->second.orientation;
197 speed = norm(firstIt->second.linearVel) * SG_METER_TO_NM * 3600.0;
199 std::vector<FGPropertyData*>::const_iterator firstPropIt;
200 std::vector<FGPropertyData*>::const_iterator firstPropItEnd;
201 firstPropIt = firstIt->second.properties.begin();
202 firstPropItEnd = firstIt->second.properties.end();
203 while (firstPropIt != firstPropItEnd) {
204 //cout << " Setting property..." << (*firstPropIt)->id;
205 PropertyMap::iterator pIt = mPropertyMap.find((*firstPropIt)->id);
206 if (pIt != mPropertyMap.end())
208 //cout << "Found " << pIt->second->getPath() << ":";
209 switch ((*firstPropIt)->type) {
213 pIt->second->setIntValue((*firstPropIt)->int_value);
214 //cout << "Int: " << (*firstPropIt)->int_value << "\n";
218 pIt->second->setFloatValue((*firstPropIt)->float_value);
219 //cout << "Flo: " << (*firstPropIt)->float_value << "\n";
222 case props::UNSPECIFIED:
223 pIt->second->setStringValue((*firstPropIt)->string_value);
224 //cout << "Str: " << (*firstPropIt)->string_value << "\n";
227 // FIXME - currently defaults to float values
228 pIt->second->setFloatValue((*firstPropIt)->float_value);
229 //cout << "Unknown: " << (*firstPropIt)->float_value << "\n";
235 SG_LOG(SG_AI, SG_DEBUG, "Unable to find property: " << (*firstPropIt)->id << "\n");
241 // Ok, we have really found something where our target time is in between
242 // do interpolation here
243 MotionInfo::iterator prevIt = nextIt;
246 // Interpolation coefficient is between 0 and 1
247 double intervalStart = prevIt->second.time;
248 double intervalEnd = nextIt->second.time;
249 double intervalLen = intervalEnd - intervalStart;
250 double tau = (tInterp - intervalStart)/intervalLen;
252 SG_LOG(SG_AI, SG_DEBUG, "Multiplayer vehicle interpolation: ["
253 << intervalStart << ", " << intervalEnd << "], intervalLen = "
254 << intervalLen << ", interpolation parameter = " << tau);
256 // Here we do just linear interpolation on the position
257 ecPos = ((1-tau)*prevIt->second.position + tau*nextIt->second.position);
258 ecOrient = interpolate((float)tau, prevIt->second.orientation,
259 nextIt->second.orientation);
260 speed = norm((1-tau)*prevIt->second.linearVel
261 + tau*nextIt->second.linearVel) * SG_METER_TO_NM * 3600.0;
263 if (prevIt->second.properties.size()
264 == nextIt->second.properties.size()) {
265 std::vector<FGPropertyData*>::const_iterator prevPropIt;
266 std::vector<FGPropertyData*>::const_iterator prevPropItEnd;
267 std::vector<FGPropertyData*>::const_iterator nextPropIt;
268 std::vector<FGPropertyData*>::const_iterator nextPropItEnd;
269 prevPropIt = prevIt->second.properties.begin();
270 prevPropItEnd = prevIt->second.properties.end();
271 nextPropIt = nextIt->second.properties.begin();
272 nextPropItEnd = nextIt->second.properties.end();
273 while (prevPropIt != prevPropItEnd) {
274 PropertyMap::iterator pIt = mPropertyMap.find((*prevPropIt)->id);
275 //cout << " Setting property..." << (*prevPropIt)->id;
277 if (pIt != mPropertyMap.end())
279 //cout << "Found " << pIt->second->getPath() << ":";
283 switch ((*prevPropIt)->type) {
287 ival = (int) (0.5+(1-tau)*((double) (*prevPropIt)->int_value) +
288 tau*((double) (*nextPropIt)->int_value));
289 pIt->second->setIntValue(ival);
290 //cout << "Int: " << ival << "\n";
294 val = (1-tau)*(*prevPropIt)->float_value +
295 tau*(*nextPropIt)->float_value;
296 //cout << "Flo: " << val << "\n";
297 pIt->second->setFloatValue(val);
300 case props::UNSPECIFIED:
301 //cout << "Str: " << (*nextPropIt)->string_value << "\n";
302 pIt->second->setStringValue((*nextPropIt)->string_value);
305 // FIXME - currently defaults to float values
306 val = (1-tau)*(*prevPropIt)->float_value +
307 tau*(*nextPropIt)->float_value;
308 //cout << "Unk: " << val << "\n";
309 pIt->second->setFloatValue(val);
315 SG_LOG(SG_AI, SG_DEBUG, "Unable to find property: " << (*prevPropIt)->id << "\n");
323 // Now throw away too old data
324 if (prevIt != mMotionInfo.begin())
327 mMotionInfo.erase(mMotionInfo.begin(), prevIt);
331 // Ok, we need to predict the future, so, take the best data we can have
332 // and do some eom computation to guess that for now.
333 FGExternalMotionData& motionInfo = it->second;
335 // The time to predict, limit to 5 seconds
336 double t = tInterp - motionInfo.time;
337 t = SGMisc<double>::min(t, 5);
339 SG_LOG(SG_AI, SG_DEBUG, "Multiplayer vehicle extrapolation: "
340 "extrapolation time = " << t);
342 // Do a few explicit euler steps with the constant acceleration's
343 // This must be sufficient ...
344 ecPos = motionInfo.position;
345 ecOrient = motionInfo.orientation;
346 SGVec3f linearVel = motionInfo.linearVel;
347 SGVec3f angularVel = motionInfo.angularVel;
353 SGVec3d ecVel = toVec3d(ecOrient.backTransform(linearVel));
355 ecOrient += h*ecOrient.derivative(angularVel);
357 linearVel += h*(cross(linearVel, angularVel) + motionInfo.linearAccel);
358 angularVel += h*motionInfo.angularAccel;
363 std::vector<FGPropertyData*>::const_iterator firstPropIt;
364 std::vector<FGPropertyData*>::const_iterator firstPropItEnd;
365 speed = norm(linearVel) * SG_METER_TO_NM * 3600.0;
366 firstPropIt = it->second.properties.begin();
367 firstPropItEnd = it->second.properties.end();
368 while (firstPropIt != firstPropItEnd) {
369 PropertyMap::iterator pIt = mPropertyMap.find((*firstPropIt)->id);
370 //cout << " Setting property..." << (*firstPropIt)->id;
372 if (pIt != mPropertyMap.end())
374 switch ((*firstPropIt)->type) {
378 pIt->second->setIntValue((*firstPropIt)->int_value);
379 //cout << "Int: " << (*firstPropIt)->int_value << "\n";
383 pIt->second->setFloatValue((*firstPropIt)->float_value);
384 //cout << "Flo: " << (*firstPropIt)->float_value << "\n";
387 case props::UNSPECIFIED:
388 pIt->second->setStringValue((*firstPropIt)->string_value);
389 //cout << "Str: " << (*firstPropIt)->string_value << "\n";
392 // FIXME - currently defaults to float values
393 pIt->second->setFloatValue((*firstPropIt)->float_value);
394 //cout << "Unk: " << (*firstPropIt)->float_value << "\n";
400 SG_LOG(SG_AI, SG_DEBUG, "Unable to find property: " << (*firstPropIt)->id << "\n");
407 // extract the position
408 pos = SGGeod::fromCart(ecPos);
409 double recent_alt_ft = altitude_ft;
410 altitude_ft = pos.getElevationFt();
412 // expose a valid vertical speed
413 if (lastUpdateTime != 0)
415 double dT = curtime - lastUpdateTime;
419 // simple smoothing over 1 second
420 vs = (1.0-Weighting)*vs + Weighting * (altitude_ft - recent_alt_ft) / dT * 60;
422 lastUpdateTime = curtime;
424 // The quaternion rotating from the earth centered frame to the
425 // horizontal local frame
426 SGQuatf qEc2Hl = SGQuatf::fromLonLatRad((float)pos.getLongitudeRad(),
427 (float)pos.getLatitudeRad());
428 // The orientation wrt the horizontal local frame
429 SGQuatf hlOr = conj(qEc2Hl)*ecOrient;
430 float hDeg, pDeg, rDeg;
431 hlOr.getEulerDeg(hDeg, pDeg, rDeg);
436 SG_LOG(SG_AI, SG_DEBUG, "Multiplayer position and orientation: "
437 << ecPos << ", " << hlOr);
439 //###########################//
440 // do calculations for radar //
441 //###########################//
442 double range_ft2 = UpdateRadar(manager);
444 //************************************//
446 //************************************//
450 //cout << "IS tanker ";
451 if ( (range_ft2 < 250.0 * 250.0) &&
454 // refuel_node->setBoolValue(true);
455 //cout << "in contact" << endl;
458 // refuel_node->setBoolValue(false);
459 //cout << "not in contact" << endl;
463 //cout << "NOT tanker " << endl;
471 FGAIMultiplayer::addMotionInfo(FGExternalMotionData& motionInfo,
474 mLastTimestamp = stamp;
476 if (!mMotionInfo.empty()) {
477 double diff = motionInfo.time - mMotionInfo.rbegin()->first;
479 // packet is very old -- MP has probably reset (incl. his timebase)
483 // drop packets arriving out of order
487 mMotionInfo[motionInfo.time] = motionInfo;
488 // We just copied the property (pointer) list - they are ours now. Clear the
489 // properties list in given/returned object, so former owner won't deallocate them.
490 motionInfo.properties.clear();
494 FGAIMultiplayer::setDoubleProperty(const std::string& prop, double val)
496 SGPropertyNode* pNode = props->getChild(prop.c_str(), true);
497 pNode->setDoubleValue(val);