src/AIModel/AIMultiplayer.cxx

   1 // FGAIMultiplayer - FGAIBase-derived class creates an AI multiplayer aircraft
   2 //
   3 // Based on FGAIAircraft
   4 // Written by David Culp, started October 2003.
   5 // Also by Gregor Richards, started December 2005.
   6 //
   7 // Copyright (C) 2003  David P. Culp - davidculp2@comcast.net
   8 // Copyright (C) 2005  Gregor Richards
   9 //
  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.
  14 //
  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.
  19 //
  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.
  23
  24 #ifdef HAVE_CONFIG_H
  25 #  include <config.h>
  26 #endif
  27
  28 #include <string>
  29 #include <stdio.h>
  30
  31 #include "AIMultiplayer.hxx"
  32
  33 using std::string;
  34
  35 // #define SG_DEBUG SG_ALERT
  36
  37 FGAIMultiplayer::FGAIMultiplayer() :
  38    FGAIBase(otMultiplayer, fgGetBool("/sim/multiplay/hot", false))
  39 {
  40    no_roll = false;
  41
  42    mTimeOffsetSet = false;
  43    mAllowExtrapolation = true;
  44    mLagAdjustSystemSpeed = 10;
  45    mLastTimestamp = 0;
  46    lastUpdateTime = 0;
  47    playerLag = 0.03;
  48    compensateLag = 1;
  49
  50 }
  51
  52 FGAIMultiplayer::~FGAIMultiplayer() {
  53 }
  54
  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
  60
  61     const string& str1 = _getCallsign();
  62     const string str2 = "MOBIL";
  63
  64     string::size_type loc1= str1.find( str2, 0 );
  65     if ( (loc1 != string::npos && str2 != "") ){
  66         //         cout << " string found "     << str2 << " in " << str1 << endl;
  67         isTanker = true;
  68         //         cout << "isTanker " << isTanker << " " << mCallSign <<endl;
  69     }
  70
  71     // load model
  72     bool result = FGAIBase::init(search_in_AI_path);
  73     // propagate installation state (used by MP pilot list)
  74     props->setBoolValue("model-installed", _installed);
  75     return result;
  76 }
  77
  78 void FGAIMultiplayer::bind() {
  79     FGAIBase::bind();
  80
  81     tie("refuel/contact", SGRawValuePointer<bool>(&contact));
  82     tie("tanker", SGRawValuePointer<bool>(&isTanker));
  83
  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);
  89
  90 #define AIMPROProp(type, name) \
  91 SGRawValueMethods<FGAIMultiplayer, type>(*this, &FGAIMultiplayer::get##name)
  92
  93 #define AIMPRWProp(type, name) \
  94 SGRawValueMethods<FGAIMultiplayer, type>(*this, \
  95       &FGAIMultiplayer::get##name, &FGAIMultiplayer::set##name)
  96
  97     //tie("callsign", AIMPROProp(const char *, CallSign));
  98
  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));
 107
 108 #undef AIMPROProp
 109 #undef AIMPRWProp
 110 }
 111
 112 void FGAIMultiplayer::update(double dt)
 113 {
 114   using namespace simgear;
 115
 116   if (dt <= 0)
 117     return;
 118
 119   FGAIBase::update(dt);
 120
 121   // Check if we already got data
 122   if (mMotionInfo.empty())
 123     return;
 124
 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();
 129
 130   // Get the last available time
 131   MotionInfo::reverse_iterator it = mMotionInfo.rbegin();
 132   double curentPkgTime = it->second.time;
 133
 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;
 147   } else {
 148           double offset = 0.0;
 149
 150                 //spectator mode, more late to be in the interpolation zone
 151     if (compensateLag == 3) { offset = curentPkgTime -curtime -lag + playerLag;
 152
 153                   // old behaviour
 154     } else if (compensateLag == 1) { offset = curentPkgTime - curtime - lag;
 155
 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;
 159     }
 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);
 165     } else {
 166       // the error of the offset, respectively the negative error to avoid
 167       // a minus later ...
 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
 172       double sysSpeed;
 173
 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) {
 177                 if (err < 0) {
 178                   sysSpeed = mLagAdjustSystemSpeed*err*0.01;
 179             } else {
 180                 sysSpeed = SGMiscd::min(0.5*err*err, 0.05);
 181                 }
 182           } else {
 183                 if (err < 0) {
 184
 185                         // Ok, we have some very late packets and nothing newer increase the
 186                         // lag by the given speedadjust
 187                         sysSpeed = mLagAdjustSystemSpeed*err;
 188                 } else {
 189                         // We have a too pessimistic display delay shorten that a small bit
 190                         sysSpeed = SGMiscd::min(0.1*err*err, 0.5);
 191            }
 192           }
 193
 194
 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;
 202
 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));
 207     }
 208   }
 209
 210
 211   // Compute the time in the feeders time scale which fits the current time
 212   // we need to
 213   double tInterp = curtime + mTimeOffset;
 214
 215   SGVec3d ecPos;
 216   SGQuatf ecOrient;
 217   SGVec3f ecLinearVel;
 218
 219   if (tInterp <= curentPkgTime) {
 220     // Ok, we need a time prevous to the last available packet,
 221     // that is good ...
 222
 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;
 233
 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())
 242         {
 243           //cout << "Found " << pIt->second->getPath() << ":";
 244           switch ((*firstPropIt)->type) {
 245             case props::INT:
 246             case props::BOOL:
 247             case props::LONG:
 248               pIt->second->setIntValue((*firstPropIt)->int_value);
 249               //cout << "Int: " << (*firstPropIt)->int_value << "\n";
 250               break;
 251             case props::FLOAT:
 252             case props::DOUBLE:
 253               pIt->second->setFloatValue((*firstPropIt)->float_value);
 254               //cout << "Flo: " << (*firstPropIt)->float_value << "\n";
 255               break;
 256             case props::STRING:
 257             case props::UNSPECIFIED:
 258               pIt->second->setStringValue((*firstPropIt)->string_value);
 259               //cout << "Str: " << (*firstPropIt)->string_value << "\n";
 260               break;
 261             default:
 262               // FIXME - currently defaults to float values
 263               pIt->second->setFloatValue((*firstPropIt)->float_value);
 264               //cout << "Unknown: " << (*firstPropIt)->float_value << "\n";
 265               break;
 266           }
 267         }
 268         else
 269         {
 270           SG_LOG(SG_AI, SG_DEBUG, "Unable to find property: " << (*firstPropIt)->id << "\n");
 271         }
 272         ++firstPropIt;
 273       }
 274
 275     } else {
 276       // Ok, we have really found something where our target time is in between
 277       // do interpolation here
 278       MotionInfo::iterator prevIt = nextIt;
 279       --prevIt;
 280
 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;
 286
 287       SG_LOG(SG_AI, SG_DEBUG, "Multiplayer vehicle interpolation: ["
 288              << intervalStart << ", " << intervalEnd << "], intervalLen = "
 289              << intervalLen << ", interpolation parameter = " << tau);
 290
 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;
 297
 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;
 311
 312           if (pIt != mPropertyMap.end())
 313           {
 314             //cout << "Found " << pIt->second->getPath() << ":";
 315
 316             int ival;
 317             float val;
 318             switch ((*prevPropIt)->type) {
 319               case props::INT:
 320               case props::BOOL:
 321               case props::LONG:
 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";
 326                 break;
 327               case props::FLOAT:
 328               case props::DOUBLE:
 329                 val = (1-tau)*(*prevPropIt)->float_value +
 330                   tau*(*nextPropIt)->float_value;
 331                 //cout << "Flo: " << val << "\n";
 332                 pIt->second->setFloatValue(val);
 333                 break;
 334               case props::STRING:
 335               case props::UNSPECIFIED:
 336                 //cout << "Str: " << (*nextPropIt)->string_value << "\n";
 337                 pIt->second->setStringValue((*nextPropIt)->string_value);
 338                 break;
 339               default:
 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);
 345                 break;
 346             }
 347           }
 348           else
 349           {
 350             SG_LOG(SG_AI, SG_DEBUG, "Unable to find property: " << (*prevPropIt)->id << "\n");
 351           }
 352
 353           ++prevPropIt;
 354           ++nextPropIt;
 355         }
 356       }
 357
 358       // Now throw away too old data
 359       if (prevIt != mMotionInfo.begin())
 360       {
 361         --prevIt;
 362         mMotionInfo.erase(mMotionInfo.begin(), prevIt);
 363       }
 364     }
 365   } else {
 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;
 369
 370     // The time to predict, limit to 3 seconds
 371     double t = tInterp - motionInfo.time;
 372     t = SGMisc<double>::min(t, 3);
 373
 374     SG_LOG(SG_AI, SG_DEBUG, "Multiplayer vehicle extrapolation: "
 375            "extrapolation time = " << t);
 376
 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));
 384
 385         double normVel = norm(ecVel);
 386
 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);
 391         }
 392
 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);
 397         } else {
 398                 ecPos += t*(ecVel);
 399         }
 400
 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;
 409
 410       if (pIt != mPropertyMap.end())
 411       {
 412         switch ((*firstPropIt)->type) {
 413           case props::INT:
 414           case props::BOOL:
 415           case props::LONG:
 416             pIt->second->setIntValue((*firstPropIt)->int_value);
 417             //cout << "Int: " << (*firstPropIt)->int_value << "\n";
 418             break;
 419           case props::FLOAT:
 420           case props::DOUBLE:
 421             pIt->second->setFloatValue((*firstPropIt)->float_value);
 422             //cout << "Flo: " << (*firstPropIt)->float_value << "\n";
 423             break;
 424           case props::STRING:
 425           case props::UNSPECIFIED:
 426             pIt->second->setStringValue((*firstPropIt)->string_value);
 427             //cout << "Str: " << (*firstPropIt)->string_value << "\n";
 428             break;
 429           default:
 430             // FIXME - currently defaults to float values
 431             pIt->second->setFloatValue((*firstPropIt)->float_value);
 432             //cout << "Unk: " << (*firstPropIt)->float_value << "\n";
 433             break;
 434         }
 435       }
 436       else
 437       {
 438         SG_LOG(SG_AI, SG_DEBUG, "Unable to find property: " << (*firstPropIt)->id << "\n");
 439       }
 440
 441       ++firstPropIt;
 442     }
 443   }
 444
 445   // extract the position
 446   pos = SGGeod::fromCart(ecPos);
 447   double recent_alt_ft = altitude_ft;
 448   altitude_ft = pos.getElevationFt();
 449
 450   // expose a valid vertical speed
 451   if (lastUpdateTime != 0)
 452   {
 453       double dT = curtime - lastUpdateTime;
 454       double Weighting=1;
 455       if (dt < 1.0)
 456           Weighting = dt;
 457       // simple smoothing over 1 second
 458       vs = (1.0-Weighting)*vs +  Weighting * (altitude_ft - recent_alt_ft) / dT * 60;
 459   }
 460   lastUpdateTime = curtime;
 461
 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);
 470   hdg = hDeg;
 471   roll = rDeg;
 472   pitch = pDeg;
 473
 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);
 478
 479   SG_LOG(SG_AI, SG_DEBUG, "Multiplayer position and orientation: "
 480          << ecPos << ", " << hlOr);
 481
 482   //###########################//
 483   // do calculations for radar //
 484   //###########################//
 485     double range_ft2 = UpdateRadar(manager);
 486
 487     //************************************//
 488     // Tanker code                        //
 489     //************************************//
 490
 491
 492     if ( isTanker) {
 493         //cout << "IS tanker ";
 494         if ( (range_ft2 < 250.0 * 250.0) &&
 495             (y_shift > 0.0)    &&
 496             (elevation > 0.0) ){
 497                 // refuel_node->setBoolValue(true);
 498                  //cout << "in contact"  << endl;
 499             contact = true;
 500         } else {
 501             // refuel_node->setBoolValue(false);
 502             //cout << "not in contact"  << endl;
 503             contact = false;
 504         }
 505     } else {
 506         //cout << "NOT tanker " << endl;
 507         contact = false;
 508     }
 509
 510   Transform();
 511 }
 512
 513 void
 514 FGAIMultiplayer::addMotionInfo(FGExternalMotionData& motionInfo,
 515                                long stamp)
 516 {
 517   mLastTimestamp = stamp;
 518
 519   if (!mMotionInfo.empty()) {
 520     double diff = motionInfo.time - mMotionInfo.rbegin()->first;
 521
 522     // packet is very old -- MP has probably reset (incl. his timebase)
 523     if (diff < -10.0)
 524       mMotionInfo.clear();
 525
 526     // drop packets arriving out of order
 527     else if (diff < 0.0)
 528       return;
 529   }
 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();
 534 }
 535
 536 void
 537 FGAIMultiplayer::setDoubleProperty(const std::string& prop, double val)
 538 {
 539   SGPropertyNode* pNode = props->getChild(prop.c_str(), true);
 540   pNode->setDoubleValue(val);
 541 }