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