X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FAIModel%2FAIMultiplayer.cxx;h=30843b7c48b3643b3a16bc9705d0035e0fdd4cac;hb=9d995907db00728da7eac9297ecbab93ed8a7400;hp=9888d58efaeb54e1417bf6ca0757a3876be1edc0;hpb=729969990305db9cd6f376ee4e65437f7eaeb8ff;p=flightgear.git diff --git a/src/AIModel/AIMultiplayer.cxx b/src/AIModel/AIMultiplayer.cxx old mode 100755 new mode 100644 index 9888d58ef..30843b7c4 --- a/src/AIModel/AIMultiplayer.cxx +++ b/src/AIModel/AIMultiplayer.cxx @@ -29,27 +29,34 @@ #include "AIMultiplayer.hxx" +using std::string; + // #define SG_DEBUG SG_ALERT -FGAIMultiplayer::FGAIMultiplayer() : FGAIBase(otMultiplayer) { +FGAIMultiplayer::FGAIMultiplayer() : + FGAIBase(otMultiplayer, false) +{ no_roll = false; mTimeOffsetSet = false; mAllowExtrapolation = true; mLagAdjustSystemSpeed = 10; -} + mLastTimestamp = 0; + lastUpdateTime = 0; +} FGAIMultiplayer::~FGAIMultiplayer() { } -bool FGAIMultiplayer::init() { +bool FGAIMultiplayer::init(bool search_in_AI_path) { + props->setStringValue("sim/model/path", model_path.c_str()); //refuel_node = fgGetNode("systems/refuel/contact", true); isTanker = false; // do this until this property is // passed over the net - string str1 = mCallSign; - string str2 = "MOBIL"; + const string& str1 = _getCallsign(); + const string str2 = "MOBIL"; string::size_type loc1= str1.find( str2, 0 ); if ( (loc1 != string::npos && str2 != "") ){ @@ -57,15 +64,26 @@ bool FGAIMultiplayer::init() { isTanker = true; // cout << "isTanker " << isTanker << " " << mCallSign <setBoolValue("model-installed", _installed); + return result; } void FGAIMultiplayer::bind() { FGAIBase::bind(); - props->tie("refuel/contact", SGRawValuePointer(&contact)); - props->setBoolValue("tanker",isTanker); + tie("refuel/contact", SGRawValuePointer(&contact)); + tie("tanker", SGRawValuePointer(&isTanker)); + tie("controls/invisible", + SGRawValuePointer(&invisible)); + _uBodyNode = props->getNode("velocities/uBody-fps", true); + _vBodyNode = props->getNode("velocities/vBody-fps", true); + _wBodyNode = props->getNode("velocities/wBody-fps", true); + #define AIMPROProp(type, name) \ SGRawValueMethods(*this, &FGAIMultiplayer::get##name) @@ -73,29 +91,22 @@ SGRawValueMethods(*this, &FGAIMultiplayer::get##name) SGRawValueMethods(*this, \ &FGAIMultiplayer::get##name, &FGAIMultiplayer::set##name) - props->tie("callsign", AIMPROProp(const char *, CallSign)); + //tie("callsign", AIMPROProp(const char *, CallSign)); - props->tie("controls/allow-extrapolation", - AIMPRWProp(bool, AllowExtrapolation)); - props->tie("controls/lag-adjust-system-speed", - AIMPRWProp(double, LagAdjustSystemSpeed)); + tie("controls/allow-extrapolation", + AIMPRWProp(bool, AllowExtrapolation)); + tie("controls/lag-adjust-system-speed", + AIMPRWProp(double, LagAdjustSystemSpeed)); #undef AIMPROProp #undef AIMPRWProp } -void FGAIMultiplayer::unbind() { - FGAIBase::unbind(); - - props->untie("callsign"); - props->untie("controls/allow-extrapolation"); - props->untie("controls/lag-adjust-system-speed"); - props->untie("refuel/contact"); -} - void FGAIMultiplayer::update(double dt) { + using namespace simgear; + if (dt <= 0) return; @@ -132,7 +143,7 @@ void FGAIMultiplayer::update(double dt) if ((!mAllowExtrapolation && offset + lag < mTimeOffset) || (offset - 10 > mTimeOffset)) { mTimeOffset = offset; - SG_LOG(SG_GENERAL, SG_DEBUG, "Resetting time offset adjust system to " + SG_LOG(SG_AI, SG_DEBUG, "Resetting time offset adjust system to " "avoid extrapolation: time offset = " << mTimeOffset); } else { // the error of the offset, respectively the negative error to avoid @@ -159,7 +170,7 @@ void FGAIMultiplayer::update(double dt) systemIncrement = err; mTimeOffset += systemIncrement; - SG_LOG(SG_GENERAL, SG_DEBUG, "Offset adjust system: time offset = " + SG_LOG(SG_AI, SG_DEBUG, "Offset adjust system: time offset = " << mTimeOffset << ", expected longitudinal position error due to " " current adjustment of the offset: " << fabs(norm(it->second.linearVel)*systemIncrement)); @@ -173,6 +184,7 @@ void FGAIMultiplayer::update(double dt) SGVec3d ecPos; SGQuatf ecOrient; + SGVec3f ecLinearVel; if (tInterp <= curentPkgTime) { // Ok, we need a time prevous to the last available packet, @@ -181,12 +193,13 @@ void FGAIMultiplayer::update(double dt) // Find the first packet before the target time MotionInfo::iterator nextIt = mMotionInfo.upper_bound(tInterp); if (nextIt == mMotionInfo.begin()) { - SG_LOG(SG_GENERAL, SG_DEBUG, "Taking oldest packet!"); + SG_LOG(SG_AI, SG_DEBUG, "Taking oldest packet!"); // We have no packet before the target time, just use the first one MotionInfo::iterator firstIt = mMotionInfo.begin(); ecPos = firstIt->second.position; ecOrient = firstIt->second.orientation; - speed = norm(firstIt->second.linearVel) * SG_METER_TO_NM * 3600.0; + ecLinearVel = firstIt->second.linearVel; + speed = norm(ecLinearVel) * SG_METER_TO_NM * 3600.0; std::vector::const_iterator firstPropIt; std::vector::const_iterator firstPropItEnd; @@ -199,19 +212,19 @@ void FGAIMultiplayer::update(double dt) { //cout << "Found " << pIt->second->getPath() << ":"; switch ((*firstPropIt)->type) { - case SGPropertyNode::INT: - case SGPropertyNode::BOOL: - case SGPropertyNode::LONG: + case props::INT: + case props::BOOL: + case props::LONG: pIt->second->setIntValue((*firstPropIt)->int_value); //cout << "Int: " << (*firstPropIt)->int_value << "\n"; break; - case SGPropertyNode::FLOAT: - case SGPropertyNode::DOUBLE: + case props::FLOAT: + case props::DOUBLE: pIt->second->setFloatValue((*firstPropIt)->float_value); //cout << "Flo: " << (*firstPropIt)->float_value << "\n"; break; - case SGPropertyNode::STRING: - case SGPropertyNode::UNSPECIFIED: + case props::STRING: + case props::UNSPECIFIED: pIt->second->setStringValue((*firstPropIt)->string_value); //cout << "Str: " << (*firstPropIt)->string_value << "\n"; break; @@ -224,7 +237,7 @@ void FGAIMultiplayer::update(double dt) } else { - SG_LOG(SG_GENERAL, SG_DEBUG, "Unable to find property: " << (*firstPropIt)->id << "\n"); + SG_LOG(SG_AI, SG_DEBUG, "Unable to find property: " << (*firstPropIt)->id << "\n"); } ++firstPropIt; } @@ -241,7 +254,7 @@ void FGAIMultiplayer::update(double dt) double intervalLen = intervalEnd - intervalStart; double tau = (tInterp - intervalStart)/intervalLen; - SG_LOG(SG_GENERAL, SG_DEBUG, "Multiplayer vehicle interpolation: [" + SG_LOG(SG_AI, SG_DEBUG, "Multiplayer vehicle interpolation: [" << intervalStart << ", " << intervalEnd << "], intervalLen = " << intervalLen << ", interpolation parameter = " << tau); @@ -249,8 +262,8 @@ void FGAIMultiplayer::update(double dt) ecPos = ((1-tau)*prevIt->second.position + tau*nextIt->second.position); ecOrient = interpolate((float)tau, prevIt->second.orientation, nextIt->second.orientation); - speed = norm((1-tau)*prevIt->second.linearVel - + tau*nextIt->second.linearVel) * SG_METER_TO_NM * 3600.0; + ecLinearVel = ((1-tau)*prevIt->second.linearVel + tau*nextIt->second.linearVel); + speed = norm(ecLinearVel) * SG_METER_TO_NM * 3600.0; if (prevIt->second.properties.size() == nextIt->second.properties.size()) { @@ -273,23 +286,23 @@ void FGAIMultiplayer::update(double dt) int ival; float val; switch ((*prevPropIt)->type) { - case SGPropertyNode::INT: - case SGPropertyNode::BOOL: - case SGPropertyNode::LONG: - ival = (int) (1-tau)*((double) (*prevPropIt)->int_value) + - tau*((double) (*nextPropIt)->int_value); + case props::INT: + case props::BOOL: + case props::LONG: + ival = (int) (0.5+(1-tau)*((double) (*prevPropIt)->int_value) + + tau*((double) (*nextPropIt)->int_value)); pIt->second->setIntValue(ival); //cout << "Int: " << ival << "\n"; break; - case SGPropertyNode::FLOAT: - case SGPropertyNode::DOUBLE: + case props::FLOAT: + case props::DOUBLE: val = (1-tau)*(*prevPropIt)->float_value + tau*(*nextPropIt)->float_value; //cout << "Flo: " << val << "\n"; pIt->second->setFloatValue(val); break; - case SGPropertyNode::STRING: - case SGPropertyNode::UNSPECIFIED: + case props::STRING: + case props::UNSPECIFIED: //cout << "Str: " << (*nextPropIt)->string_value << "\n"; pIt->second->setStringValue((*nextPropIt)->string_value); break; @@ -304,7 +317,7 @@ void FGAIMultiplayer::update(double dt) } else { - SG_LOG(SG_GENERAL, SG_DEBUG, "Unable to find property: " << (*prevPropIt)->id << "\n"); + SG_LOG(SG_AI, SG_DEBUG, "Unable to find property: " << (*prevPropIt)->id << "\n"); } ++prevPropIt; @@ -316,59 +329,37 @@ void FGAIMultiplayer::update(double dt) if (prevIt != mMotionInfo.begin()) { --prevIt; - - MotionInfo::iterator delIt; - delIt = mMotionInfo.begin(); - - while (delIt != prevIt) - { - std::vector::const_iterator propIt; - std::vector::const_iterator propItEnd; - propIt = delIt->second.properties.begin(); - propItEnd = delIt->second.properties.end(); - - //cout << "Deleting data\n"; - - while (propIt != propItEnd) - { - delete *propIt; - propIt++; - } - - delIt++; - } - mMotionInfo.erase(mMotionInfo.begin(), prevIt); } } } else { // Ok, we need to predict the future, so, take the best data we can have // and do some eom computation to guess that for now. - FGExternalMotionData motionInfo = it->second; + FGExternalMotionData& motionInfo = it->second; // The time to predict, limit to 5 seconds double t = tInterp - motionInfo.time; t = SGMisc::min(t, 5); - SG_LOG(SG_GENERAL, SG_DEBUG, "Multiplayer vehicle extrapolation: " + SG_LOG(SG_AI, SG_DEBUG, "Multiplayer vehicle extrapolation: " "extrapolation time = " << t); // Do a few explicit euler steps with the constant acceleration's // This must be sufficient ... ecPos = motionInfo.position; ecOrient = motionInfo.orientation; - SGVec3f linearVel = motionInfo.linearVel; + ecLinearVel = motionInfo.linearVel; SGVec3f angularVel = motionInfo.angularVel; while (0 < t) { double h = 1e-1; if (t < h) h = t; - SGVec3d ecVel = toVec3d(ecOrient.backTransform(linearVel)); + SGVec3d ecVel = toVec3d(ecOrient.backTransform(ecLinearVel)); ecPos += h*ecVel; ecOrient += h*ecOrient.derivative(angularVel); - linearVel += h*(cross(linearVel, angularVel) + motionInfo.linearAccel); + ecLinearVel += h*(cross(ecLinearVel, angularVel) + motionInfo.linearAccel); angularVel += h*motionInfo.angularAccel; t -= h; @@ -376,7 +367,7 @@ void FGAIMultiplayer::update(double dt) std::vector::const_iterator firstPropIt; std::vector::const_iterator firstPropItEnd; - speed = norm(linearVel) * SG_METER_TO_NM * 3600.0; + speed = norm(ecLinearVel) * SG_METER_TO_NM * 3600.0; firstPropIt = it->second.properties.begin(); firstPropItEnd = it->second.properties.end(); while (firstPropIt != firstPropItEnd) { @@ -386,19 +377,19 @@ void FGAIMultiplayer::update(double dt) if (pIt != mPropertyMap.end()) { switch ((*firstPropIt)->type) { - case SGPropertyNode::INT: - case SGPropertyNode::BOOL: - case SGPropertyNode::LONG: + case props::INT: + case props::BOOL: + case props::LONG: pIt->second->setIntValue((*firstPropIt)->int_value); //cout << "Int: " << (*firstPropIt)->int_value << "\n"; break; - case SGPropertyNode::FLOAT: - case SGPropertyNode::DOUBLE: + case props::FLOAT: + case props::DOUBLE: pIt->second->setFloatValue((*firstPropIt)->float_value); //cout << "Flo: " << (*firstPropIt)->float_value << "\n"; break; - case SGPropertyNode::STRING: - case SGPropertyNode::UNSPECIFIED: + case props::STRING: + case props::UNSPECIFIED: pIt->second->setStringValue((*firstPropIt)->string_value); //cout << "Str: " << (*firstPropIt)->string_value << "\n"; break; @@ -411,7 +402,7 @@ void FGAIMultiplayer::update(double dt) } else { - SG_LOG(SG_GENERAL, SG_DEBUG, "Unable to find property: " << (*firstPropIt)->id << "\n"); + SG_LOG(SG_AI, SG_DEBUG, "Unable to find property: " << (*firstPropIt)->id << "\n"); } ++firstPropIt; @@ -420,8 +411,21 @@ void FGAIMultiplayer::update(double dt) // extract the position pos = SGGeod::fromCart(ecPos); + double recent_alt_ft = altitude_ft; altitude_ft = pos.getElevationFt(); + // expose a valid vertical speed + if (lastUpdateTime != 0) + { + double dT = curtime - lastUpdateTime; + double Weighting=1; + if (dt < 1.0) + Weighting = dt; + // simple smoothing over 1 second + vs = (1.0-Weighting)*vs + Weighting * (altitude_ft - recent_alt_ft) / dT * 60; + } + lastUpdateTime = curtime; + // The quaternion rotating from the earth centered frame to the // horizontal local frame SGQuatf qEc2Hl = SGQuatf::fromLonLatRad((float)pos.getLongitudeRad(), @@ -434,7 +438,12 @@ void FGAIMultiplayer::update(double dt) roll = rDeg; pitch = pDeg; - SG_LOG(SG_GENERAL, SG_DEBUG, "Multiplayer position and orientation: " + // expose velocities/u,v,wbody-fps in the mp tree + _uBodyNode->setValue(ecLinearVel[0] * SG_METER_TO_FEET); + _vBodyNode->setValue(ecLinearVel[1] * SG_METER_TO_FEET); + _wBodyNode->setValue(ecLinearVel[2] * SG_METER_TO_FEET); + + SG_LOG(SG_AI, SG_DEBUG, "Multiplayer position and orientation: " << ecPos << ", " << hlOr); //###########################// @@ -448,16 +457,20 @@ void FGAIMultiplayer::update(double dt) if ( isTanker) { + //cout << "IS tanker "; if ( (range_ft2 < 250.0 * 250.0) && (y_shift > 0.0) && (elevation > 0.0) ){ // refuel_node->setBoolValue(true); + //cout << "in contact" << endl; contact = true; } else { // refuel_node->setBoolValue(false); + //cout << "not in contact" << endl; contact = false; } } else { + //cout << "NOT tanker " << endl; contact = false; } @@ -465,14 +478,26 @@ void FGAIMultiplayer::update(double dt) } void -FGAIMultiplayer::addMotionInfo(const FGExternalMotionData& motionInfo, +FGAIMultiplayer::addMotionInfo(FGExternalMotionData& motionInfo, long stamp) { mLastTimestamp = stamp; - // Drop packets arriving out of order - if (!mMotionInfo.empty() && motionInfo.time < mMotionInfo.rbegin()->first) - return; + + if (!mMotionInfo.empty()) { + double diff = motionInfo.time - mMotionInfo.rbegin()->first; + + // packet is very old -- MP has probably reset (incl. his timebase) + if (diff < -10.0) + mMotionInfo.clear(); + + // drop packets arriving out of order + else if (diff < 0.0) + return; + } mMotionInfo[motionInfo.time] = motionInfo; + // We just copied the property (pointer) list - they are ours now. Clear the + // properties list in given/returned object, so former owner won't deallocate them. + motionInfo.properties.clear(); } void