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