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 <Main/globals.hxx>
  32 #include <Main/fg_props.hxx>
  33
  34 #include "AIMultiplayer.hxx"
  35
  36 using std::string;
  37
  38 // #define SG_DEBUG SG_ALERT
  39
  40 FGAIMultiplayer::FGAIMultiplayer() :
  41    FGAIBase(otMultiplayer, fgGetBool("/sim/multiplay/hot", false))
  42 {
  43    no_roll = false;
  44
  45    mTimeOffsetSet = false;
  46    mAllowExtrapolation = true;
  47    mLagAdjustSystemSpeed = 10;
  48    mLastTimestamp = 0;
  49    lastUpdateTime = 0;
  50    playerLag = 0.03;
  51    compensateLag = 1;
  52
  53 }
  54
  55 FGAIMultiplayer::~FGAIMultiplayer() {
  56 }
  57
  58 bool FGAIMultiplayer::init(bool search_in_AI_path) {
  59     props->setStringValue("sim/model/path", model_path.c_str());
  60     //refuel_node = fgGetNode("systems/refuel/contact", true);
  61     isTanker = false; // do this until this property is
  62                       // passed over the net
  63
  64     const string& str1 = _getCallsign();
  65     const string str2 = "MOBIL";
  66
  67     string::size_type loc1= str1.find( str2, 0 );
  68     if ( (loc1 != string::npos && str2 != "") ){
  69         //         cout << " string found "     << str2 << " in " << str1 << endl;
  70         isTanker = true;
  71         //         cout << "isTanker " << isTanker << " " << mCallSign <<endl;
  72     }
  73
  74     // load model
  75     bool result = FGAIBase::init(search_in_AI_path);
  76     // propagate installation state (used by MP pilot list)
  77     props->setBoolValue("model-installed", _installed);
  78     return result;
  79 }
  80
  81 void FGAIMultiplayer::bind() {
  82     FGAIBase::bind();
  83
  84     tie("refuel/contact", SGRawValuePointer<bool>(&contact));
  85     tie("tanker", SGRawValuePointer<bool>(&isTanker));
  86
  87     tie("controls/invisible",
  88         SGRawValuePointer<bool>(&invisible));
  89         _uBodyNode = props->getNode("velocities/uBody-fps", true);
  90         _vBodyNode = props->getNode("velocities/vBody-fps", true);
  91         _wBodyNode = props->getNode("velocities/wBody-fps", true);
  92
  93 #define AIMPROProp(type, name) \
  94 SGRawValueMethods<FGAIMultiplayer, type>(*this, &FGAIMultiplayer::get##name)
  95
  96 #define AIMPRWProp(type, name) \
  97 SGRawValueMethods<FGAIMultiplayer, type>(*this, \
  98       &FGAIMultiplayer::get##name, &FGAIMultiplayer::set##name)
  99
 100     //tie("callsign", AIMPROProp(const char *, CallSign));
 101
 102     tie("controls/allow-extrapolation",
 103         AIMPRWProp(bool, AllowExtrapolation));
 104     tie("controls/lag-adjust-system-speed",
 105         AIMPRWProp(double, LagAdjustSystemSpeed));
 106     tie("controls/player-lag",
 107         AIMPRWProp(double, playerLag));
 108     tie("controls/compensate-lag",
 109         AIMPRWProp(int, compensateLag));
 110
 111 #undef AIMPROProp
 112 #undef AIMPRWProp
 113 }
 114
 115 void FGAIMultiplayer::update(double dt)
 116 {
 117   using namespace simgear;
 118
 119   if (dt <= 0)
 120     return;
 121
 122   FGAIBase::update(dt);
 123
 124   // Check if we already got data
 125   if (mMotionInfo.empty())
 126     return;
 127
 128   // The current simulation time we need to update for,
 129   // note that the simulation time is updated before calling all the
 130   // update methods. Thus it contains the time intervals *end* time
 131   double curtime = globals->get_sim_time_sec();
 132
 133   // Get the last available time
 134   MotionInfo::reverse_iterator it = mMotionInfo.rbegin();
 135   double curentPkgTime = it->second.time;
 136
 137   // Dynamically optimize the time offset between the feeder and the client
 138   // Well, 'dynamically' means that the dynamic of that update must be very
 139   // slow. You would otherwise notice huge jumps in the multiplayer models.
 140   // The reason is that we want to avoid huge extrapolation times since
 141   // extrapolation is highly error prone. For that we need something
 142   // approaching the average latency of the packets. This first order lag
 143   // component will provide this. We just take the error of the currently
 144   // requested time to the most recent available packet. This is the
 145   // target we want to reach in average.
 146   double lag = it->second.lag;
 147   if (!mTimeOffsetSet) {
 148     mTimeOffsetSet = true;
 149     mTimeOffset = curentPkgTime - curtime - lag;
 150   } else {
 151           double offset = 0.0;
 152
 153                 //spectator mode, more late to be in the interpolation zone
 154     if (compensateLag == 3) { offset = curentPkgTime -curtime -lag + playerLag;
 155
 156                   // old behaviour
 157     } else if (compensateLag == 1) { offset = curentPkgTime - curtime - lag;
 158
 159     // using the prediction mode to display the mpaircraft in the futur/past with given playerlag value
 160     //currently compensatelag = 2
 161     } else { offset = curentPkgTime - curtime + 0.48*lag + playerLag;
 162     }
 163     if ((!mAllowExtrapolation && offset + lag < mTimeOffset)
 164         || (offset - 10 > mTimeOffset)) {
 165       mTimeOffset = offset;
 166       SG_LOG(SG_AI, SG_DEBUG, "Resetting time offset adjust system to "
 167              "avoid extrapolation: time offset = " << mTimeOffset);
 168     } else {
 169       // the error of the offset, respectively the negative error to avoid
 170       // a minus later ...
 171       double err = offset - mTimeOffset;
 172       // limit errors leading to shorter lag values somehow, that is late
 173       // arriving packets will pessimize the overall lag much more than
 174       // early packets will shorten the overall lag
 175       double sysSpeed;
 176
 177   //trying to slow the rudderlag phenomenon thus using more the prediction system
 178   //if we are off by less than 1.5s, do a little correction, and bigger step above 1.5s
 179           if (fabs(err) < 1.5) {
 180                 if (err < 0) {
 181                   sysSpeed = mLagAdjustSystemSpeed*err*0.01;
 182             } else {
 183                 sysSpeed = SGMiscd::min(0.5*err*err, 0.05);
 184                 }
 185           } else {
 186                 if (err < 0) {
 187
 188                         // Ok, we have some very late packets and nothing newer increase the
 189                         // lag by the given speedadjust
 190                         sysSpeed = mLagAdjustSystemSpeed*err;
 191                 } else {
 192                         // We have a too pessimistic display delay shorten that a small bit
 193                         sysSpeed = SGMiscd::min(0.1*err*err, 0.5);
 194            }
 195           }
 196
 197
 198       // simple euler integration for that first order system including some
 199       // overshooting guard to prevent to aggressive system speeds
 200       // (stiff systems) to explode the systems state
 201       double systemIncrement = dt*sysSpeed;
 202       if (fabs(err) < fabs(systemIncrement))
 203         systemIncrement = err;
 204       mTimeOffset += systemIncrement;
 205
 206       SG_LOG(SG_AI, SG_DEBUG, "Offset adjust system: time offset = "
 207              << mTimeOffset << ", expected longitudinal position error due to "
 208              " current adjustment of the offset: "
 209              << fabs(norm(it->second.linearVel)*systemIncrement));
 210     }
 211   }
 212
 213
 214   // Compute the time in the feeders time scale which fits the current time
 215   // we need to
 216   double tInterp = curtime + mTimeOffset;
 217
 218   SGVec3d ecPos;
 219   SGQuatf ecOrient;
 220   SGVec3f ecLinearVel;
 221
 222   if (tInterp <= curentPkgTime) {
 223     // Ok, we need a time prevous to the last available packet,
 224     // that is good ...
 225
 226     // Find the first packet before the target time
 227     MotionInfo::iterator nextIt = mMotionInfo.upper_bound(tInterp);
 228     if (nextIt == mMotionInfo.begin()) {
 229       SG_LOG(SG_AI, SG_DEBUG, "Taking oldest packet!");
 230       // We have no packet before the target time, just use the first one
 231       MotionInfo::iterator firstIt = mMotionInfo.begin();
 232       ecPos = firstIt->second.position;
 233       ecOrient = firstIt->second.orientation;
 234       ecLinearVel = firstIt->second.linearVel;
 235       speed = norm(ecLinearVel) * SG_METER_TO_NM * 3600.0;
 236
 237       std::vector<FGPropertyData*>::const_iterator firstPropIt;
 238       std::vector<FGPropertyData*>::const_iterator firstPropItEnd;
 239       firstPropIt = firstIt->second.properties.begin();
 240       firstPropItEnd = firstIt->second.properties.end();
 241       while (firstPropIt != firstPropItEnd) {
 242         //cout << " Setting property..." << (*firstPropIt)->id;
 243         PropertyMap::iterator pIt = mPropertyMap.find((*firstPropIt)->id);
 244         if (pIt != mPropertyMap.end())
 245         {
 246           //cout << "Found " << pIt->second->getPath() << ":";
 247           switch ((*firstPropIt)->type) {
 248             case props::INT:
 249             case props::BOOL:
 250             case props::LONG:
 251               pIt->second->setIntValue((*firstPropIt)->int_value);
 252               //cout << "Int: " << (*firstPropIt)->int_value << "\n";
 253               break;
 254             case props::FLOAT:
 255             case props::DOUBLE:
 256               pIt->second->setFloatValue((*firstPropIt)->float_value);
 257               //cout << "Flo: " << (*firstPropIt)->float_value << "\n";
 258               break;
 259             case props::STRING:
 260             case props::UNSPECIFIED:
 261               pIt->second->setStringValue((*firstPropIt)->string_value);
 262               //cout << "Str: " << (*firstPropIt)->string_value << "\n";
 263               break;
 264             default:
 265               // FIXME - currently defaults to float values
 266               pIt->second->setFloatValue((*firstPropIt)->float_value);
 267               //cout << "Unknown: " << (*firstPropIt)->float_value << "\n";
 268               break;
 269           }
 270         }
 271         else
 272         {
 273           SG_LOG(SG_AI, SG_DEBUG, "Unable to find property: " << (*firstPropIt)->id << "\n");
 274         }
 275         ++firstPropIt;
 276       }
 277
 278     } else {
 279       // Ok, we have really found something where our target time is in between
 280       // do interpolation here
 281       MotionInfo::iterator prevIt = nextIt;
 282       --prevIt;
 283
 284       // Interpolation coefficient is between 0 and 1
 285       double intervalStart = prevIt->second.time;
 286       double intervalEnd = nextIt->second.time;
 287       double intervalLen = intervalEnd - intervalStart;
 288       double tau = (tInterp - intervalStart)/intervalLen;
 289
 290       SG_LOG(SG_AI, SG_DEBUG, "Multiplayer vehicle interpolation: ["
 291              << intervalStart << ", " << intervalEnd << "], intervalLen = "
 292              << intervalLen << ", interpolation parameter = " << tau);
 293
 294       // Here we do just linear interpolation on the position
 295       ecPos = ((1-tau)*prevIt->second.position + tau*nextIt->second.position);
 296       ecOrient = interpolate((float)tau, prevIt->second.orientation,
 297                              nextIt->second.orientation);
 298       ecLinearVel = ((1-tau)*prevIt->second.linearVel + tau*nextIt->second.linearVel);
 299       speed = norm(ecLinearVel) * SG_METER_TO_NM * 3600.0;
 300
 301       if (prevIt->second.properties.size()
 302           == nextIt->second.properties.size()) {
 303         std::vector<FGPropertyData*>::const_iterator prevPropIt;
 304         std::vector<FGPropertyData*>::const_iterator prevPropItEnd;
 305         std::vector<FGPropertyData*>::const_iterator nextPropIt;
 306         std::vector<FGPropertyData*>::const_iterator nextPropItEnd;
 307         prevPropIt = prevIt->second.properties.begin();
 308         prevPropItEnd = prevIt->second.properties.end();
 309         nextPropIt = nextIt->second.properties.begin();
 310         nextPropItEnd = nextIt->second.properties.end();
 311         while (prevPropIt != prevPropItEnd) {
 312           PropertyMap::iterator pIt = mPropertyMap.find((*prevPropIt)->id);
 313           //cout << " Setting property..." << (*prevPropIt)->id;
 314
 315           if (pIt != mPropertyMap.end())
 316           {
 317             //cout << "Found " << pIt->second->getPath() << ":";
 318
 319             int ival;
 320             float val;
 321             switch ((*prevPropIt)->type) {
 322               case props::INT:
 323               case props::BOOL:
 324               case props::LONG:
 325                 ival = (int) (0.5+(1-tau)*((double) (*prevPropIt)->int_value) +
 326                   tau*((double) (*nextPropIt)->int_value));
 327                 pIt->second->setIntValue(ival);
 328                 //cout << "Int: " << ival << "\n";
 329                 break;
 330               case props::FLOAT:
 331               case props::DOUBLE:
 332                 val = (1-tau)*(*prevPropIt)->float_value +
 333                   tau*(*nextPropIt)->float_value;
 334                 //cout << "Flo: " << val << "\n";
 335                 pIt->second->setFloatValue(val);
 336                 break;
 337               case props::STRING:
 338               case props::UNSPECIFIED:
 339                 //cout << "Str: " << (*nextPropIt)->string_value << "\n";
 340                 pIt->second->setStringValue((*nextPropIt)->string_value);
 341                 break;
 342               default:
 343                 // FIXME - currently defaults to float values
 344                 val = (1-tau)*(*prevPropIt)->float_value +
 345                   tau*(*nextPropIt)->float_value;
 346                 //cout << "Unk: " << val << "\n";
 347                 pIt->second->setFloatValue(val);
 348                 break;
 349             }
 350           }
 351           else
 352           {
 353             SG_LOG(SG_AI, SG_DEBUG, "Unable to find property: " << (*prevPropIt)->id << "\n");
 354           }
 355
 356           ++prevPropIt;
 357           ++nextPropIt;
 358         }
 359       }
 360
 361       // Now throw away too old data
 362       if (prevIt != mMotionInfo.begin())
 363       {
 364         --prevIt;
 365         mMotionInfo.erase(mMotionInfo.begin(), prevIt);
 366       }
 367     }
 368   } else {
 369     // Ok, we need to predict the future, so, take the best data we can have
 370     // and do some eom computation to guess that for now.
 371     FGExternalMotionData& motionInfo = it->second;
 372
 373     // The time to predict, limit to 3 seconds
 374     double t = tInterp - motionInfo.time;
 375     t = SGMisc<double>::min(t, 3);
 376
 377     SG_LOG(SG_AI, SG_DEBUG, "Multiplayer vehicle extrapolation: "
 378            "extrapolation time = " << t);
 379
 380     // using velocity and acceleration to guess a parabolic position...
 381     ecPos = motionInfo.position;
 382     ecOrient = motionInfo.orientation;
 383     ecLinearVel = motionInfo.linearVel;
 384     SGVec3d ecVel = toVec3d(ecOrient.backTransform(ecLinearVel));
 385     SGVec3f angularVel = motionInfo.angularVel;
 386     SGVec3d ecAcc = toVec3d(ecOrient.backTransform(motionInfo.linearAccel));
 387
 388         double normVel = norm(ecVel);
 389
 390         // not doing rotationnal prediction for small speed or rotation rate,
 391         // to avoid agitated parked plane
 392     if (( norm(angularVel) > 0.05 ) || ( normVel > 1.0 )) {
 393                 ecOrient += t*ecOrient.derivative(angularVel);
 394         }
 395
 396         // not using acceleration for small speed, to have better parked planes
 397         // note that anyway acceleration is not transmit yet by mp
 398         if ( normVel > 1.0 ) {
 399                 ecPos += t*(ecVel + 0.5*t*ecAcc);
 400         } else {
 401                 ecPos += t*(ecVel);
 402         }
 403
 404         std::vector<FGPropertyData*>::const_iterator firstPropIt;
 405     std::vector<FGPropertyData*>::const_iterator firstPropItEnd;
 406     speed = norm(ecLinearVel) * SG_METER_TO_NM * 3600.0;
 407     firstPropIt = it->second.properties.begin();
 408     firstPropItEnd = it->second.properties.end();
 409     while (firstPropIt != firstPropItEnd) {
 410       PropertyMap::iterator pIt = mPropertyMap.find((*firstPropIt)->id);
 411       //cout << " Setting property..." << (*firstPropIt)->id;
 412
 413       if (pIt != mPropertyMap.end())
 414       {
 415         switch ((*firstPropIt)->type) {
 416           case props::INT:
 417           case props::BOOL:
 418           case props::LONG:
 419             pIt->second->setIntValue((*firstPropIt)->int_value);
 420             //cout << "Int: " << (*firstPropIt)->int_value << "\n";
 421             break;
 422           case props::FLOAT:
 423           case props::DOUBLE:
 424             pIt->second->setFloatValue((*firstPropIt)->float_value);
 425             //cout << "Flo: " << (*firstPropIt)->float_value << "\n";
 426             break;
 427           case props::STRING:
 428           case props::UNSPECIFIED:
 429             pIt->second->setStringValue((*firstPropIt)->string_value);
 430             //cout << "Str: " << (*firstPropIt)->string_value << "\n";
 431             break;
 432           default:
 433             // FIXME - currently defaults to float values
 434             pIt->second->setFloatValue((*firstPropIt)->float_value);
 435             //cout << "Unk: " << (*firstPropIt)->float_value << "\n";
 436             break;
 437         }
 438       }
 439       else
 440       {
 441         SG_LOG(SG_AI, SG_DEBUG, "Unable to find property: " << (*firstPropIt)->id << "\n");
 442       }
 443
 444       ++firstPropIt;
 445     }
 446   }
 447
 448   // extract the position
 449   pos = SGGeod::fromCart(ecPos);
 450   double recent_alt_ft = altitude_ft;
 451   altitude_ft = pos.getElevationFt();
 452
 453   // expose a valid vertical speed
 454   if (lastUpdateTime != 0)
 455   {
 456       double dT = curtime - lastUpdateTime;
 457       double Weighting=1;
 458       if (dt < 1.0)
 459           Weighting = dt;
 460       // simple smoothing over 1 second
 461       vs = (1.0-Weighting)*vs +  Weighting * (altitude_ft - recent_alt_ft) / dT * 60;
 462   }
 463   lastUpdateTime = curtime;
 464
 465   // The quaternion rotating from the earth centered frame to the
 466   // horizontal local frame
 467   SGQuatf qEc2Hl = SGQuatf::fromLonLatRad((float)pos.getLongitudeRad(),
 468                                           (float)pos.getLatitudeRad());
 469   // The orientation wrt the horizontal local frame
 470   SGQuatf hlOr = conj(qEc2Hl)*ecOrient;
 471   float hDeg, pDeg, rDeg;
 472   hlOr.getEulerDeg(hDeg, pDeg, rDeg);
 473   hdg = hDeg;
 474   roll = rDeg;
 475   pitch = pDeg;
 476
 477   // expose velocities/u,v,wbody-fps in the mp tree
 478   _uBodyNode->setValue(ecLinearVel[0] * SG_METER_TO_FEET);
 479   _vBodyNode->setValue(ecLinearVel[1] * SG_METER_TO_FEET);
 480   _wBodyNode->setValue(ecLinearVel[2] * SG_METER_TO_FEET);
 481
 482   SG_LOG(SG_AI, SG_DEBUG, "Multiplayer position and orientation: "
 483          << ecPos << ", " << hlOr);
 484
 485   //###########################//
 486   // do calculations for radar //
 487   //###########################//
 488     double range_ft2 = UpdateRadar(manager);
 489
 490     //************************************//
 491     // Tanker code                        //
 492     //************************************//
 493
 494
 495     if ( isTanker) {
 496         //cout << "IS tanker ";
 497         if ( (range_ft2 < 250.0 * 250.0) &&
 498             (y_shift > 0.0)    &&
 499             (elevation > 0.0) ){
 500                 // refuel_node->setBoolValue(true);
 501                  //cout << "in contact"  << endl;
 502             contact = true;
 503         } else {
 504             // refuel_node->setBoolValue(false);
 505             //cout << "not in contact"  << endl;
 506             contact = false;
 507         }
 508     } else {
 509         //cout << "NOT tanker " << endl;
 510         contact = false;
 511     }
 512
 513   Transform();
 514 }
 515
 516 void
 517 FGAIMultiplayer::addMotionInfo(FGExternalMotionData& motionInfo,
 518                                long stamp)
 519 {
 520   mLastTimestamp = stamp;
 521
 522   if (!mMotionInfo.empty()) {
 523     double diff = motionInfo.time - mMotionInfo.rbegin()->first;
 524
 525     // packet is very old -- MP has probably reset (incl. his timebase)
 526     if (diff < -10.0)
 527       mMotionInfo.clear();
 528
 529     // drop packets arriving out of order
 530     else if (diff < 0.0)
 531       return;
 532   }
 533   mMotionInfo[motionInfo.time] = motionInfo;
 534   // We just copied the property (pointer) list - they are ours now. Clear the
 535   // properties list in given/returned object, so former owner won't deallocate them.
 536   motionInfo.properties.clear();
 537 }
 538
 539 void
 540 FGAIMultiplayer::setDoubleProperty(const std::string& prop, double val)
 541 {
 542   SGPropertyNode* pNode = props->getChild(prop.c_str(), true);
 543   pNode->setDoubleValue(val);
 544 }