From 10866851de802979af4a5857aa795d0b4a90b0c5 Mon Sep 17 00:00:00 2001 From: Vivian Meazza Date: Mon, 27 Sep 2010 23:50:44 +0100 Subject: [PATCH] Add join manoeuvre/command to AIWingman. Rationalize more methods. Fix some more bugs --- src/AIModel/AIBallistic.cxx | 134 +++--------- src/AIModel/AIBallistic.hxx | 82 +++++--- src/AIModel/AIBase.cxx | 1 + src/AIModel/AIBase.hxx | 45 ++++ src/AIModel/AIEscort.cxx | 30 --- src/AIModel/AIEscort.hxx | 10 +- src/AIModel/AIManager.cxx | 1 + src/AIModel/AIManager.hxx | 9 +- src/AIModel/AIWingman.cxx | 408 +++++++++++++++++++++++++++++++++++- src/AIModel/AIWingman.hxx | 104 +++++++++ 10 files changed, 646 insertions(+), 178 deletions(-) diff --git a/src/AIModel/AIBallistic.cxx b/src/AIModel/AIBallistic.cxx index 261885b24..2ffaad6a5 100644 --- a/src/AIModel/AIBallistic.cxx +++ b/src/AIModel/AIBallistic.cxx @@ -45,7 +45,6 @@ _ht_agl_ft(0.0), _azimuth(0.0), _elevation(0.0), _rotation(0.0), -_formate_to_ac(false), _aero_stabilised(false), _drag_area(0.007), _life_timer(0.0), @@ -67,6 +66,7 @@ _external_force(false), _impact_report_node(fgGetNode("/ai/models/model-impact", true)), _old_height(0), _elapsed_time(0), +_speed(), hs(0) { @@ -228,13 +228,13 @@ void FGAIBallistic::bind() { (*this, &FGAIBallistic::getLoadOffset, &FGAIBallistic::setLoadOffset)); props->tie("load/distance-to-hitch-ft", SGRawValueMethods - (*this, &FGAIBallistic::getDistanceLoadToHitch)); + (*this, &FGAIBallistic::getDistanceToHitch)); props->tie("load/elevation-to-hitch-deg", SGRawValueMethods - (*this, &FGAIBallistic::getElevLoadToHitch)); + (*this, &FGAIBallistic::getElevToHitch)); props->tie("load/bearing-to-hitch-deg", SGRawValueMethods - (*this, &FGAIBallistic::getBearingLoadToHitch)); + (*this, &FGAIBallistic::getBearingToHitch)); props->tie("material/load-resistance", SGRawValuePointer(&_load_resistance)); } @@ -278,10 +278,7 @@ void FGAIBallistic::update(double dt) { FGAIBase::update(dt); _setUserPos(); - if (_formate_to_ac){ - formateToAC(dt); - Transform(); - } else if (_slave_to_ac){ + if (_slave_to_ac){ slaveToAC(dt); Transform(); } else if (!invisible){ @@ -425,10 +422,6 @@ void FGAIBallistic::setSlaved(bool s) { _slave_to_ac = s; } -void FGAIBallistic::setFormate(bool f) { - _formate_to_ac = f; -} - void FGAIBallistic::setContentsPath(const string& path) { _contents_path = path; @@ -471,6 +464,8 @@ void FGAIBallistic::setParentNodes(SGPropertyNode_ptr node) { void FGAIBallistic::setParentPos() { if (_pnode != 0) { + //cout << "set parent pos" << endl; + double lat = _p_lat_node->getDoubleValue(); double lon = _p_lon_node->getDoubleValue(); double alt = _p_alt_node->getDoubleValue(); @@ -487,10 +482,6 @@ bool FGAIBallistic::getSlaved() const { return _slave_to_ac; } -bool FGAIBallistic::getFormate() const { - return _formate_to_ac; -} - double FGAIBallistic::getMass() const { return _mass; } @@ -578,22 +569,31 @@ void FGAIBallistic::setBnk(double r, double dt, double coeff){ roll = (r * c) + (roll * (1 - c)); } +void FGAIBallistic::setSpd(double s, double dt, double coeff){ + double c = dt / (coeff + dt); + _speed = (s * c) + (_speed * (1 - c)); +} + void FGAIBallistic::setHt(double h, double dt, double coeff){ double c = dt / (coeff + dt); _height = (h * c) + (_height * (1 - c)); } -void FGAIBallistic::setHdg(double az, double dt, double coeff){ +int FGAIBallistic::setHdg(double tgt_hdg, double dt, double coeff){ double recip = getRecip(hdg); double c = dt / (coeff + dt); //we need to ensure that we turn the short way to the new hdg - if (az < recip && az < hdg && hdg > 180) { - hdg = ((az + 360) * c) + (hdg * (1 - c)); - } else if (az > recip && az > hdg && hdg <= 180){ - hdg = ((az - 360) * c) + (hdg * (1 - c)); + if (tgt_hdg < recip && tgt_hdg < hdg && hdg > 180) { + hdg = ((tgt_hdg + 360) * c) + (hdg * (1 - c)); +// cout << "case 1: right turn" << endl; + } else if (tgt_hdg > recip && tgt_hdg > hdg && hdg <= 180){ + hdg = ((tgt_hdg - 360) * c) + (hdg * (1 - c)); +// cout << "case 2: left turn" << endl; } else { - hdg = (az * c) + (hdg * (1 - c)); + hdg = (tgt_hdg * c) + (hdg * (1 - c)); +// cout << "case 4: left turn" << endl; } + return -1; } double FGAIBallistic::getTgtXOffset() const { @@ -1064,7 +1064,7 @@ void FGAIBallistic::setOffsetPos(SGGeod inpos, double heading, double pitch, dou } -double FGAIBallistic::getDistanceLoadToHitch() const { +double FGAIBallistic::getDistanceToHitch() const { //calculate the distance load to hitch SGVec3d carthitchPos = getCartHitchPos(); SGVec3d cartPos = getCartPos(); @@ -1074,10 +1074,9 @@ double FGAIBallistic::getDistanceLoadToHitch() const { return distance * SG_METER_TO_FEET; } - -double FGAIBallistic::getElevLoadToHitch() const { +double FGAIBallistic::getElevToHitch() const { // now the angle, positive angles are upwards - double distance = getDistanceLoadToHitch() * SG_FEET_TO_METER; + double distance = getDistanceToHitch() * SG_FEET_TO_METER; double angle = 0; double daltM = _offsetpos.getElevationM() - pos.getElevationM(); @@ -1092,9 +1091,10 @@ double FGAIBallistic::getElevLoadToHitch() const { return angle; } -double FGAIBallistic::getBearingLoadToHitch() const { +double FGAIBallistic::getBearingToHitch() const { //calculate the bearing and range of the second pos from the first - double az1, az2, distance; + double distance = getDistanceToHitch() * SG_FEET_TO_METER; + double az1, az2; geo_inverse_wgs_84(pos, _offsetpos, &az1, &az2, &distance); @@ -1109,8 +1109,7 @@ double FGAIBallistic::getRelBrgHitchToUser() const { double rel_brg = az1 - hdg; - if (rel_brg > 180) - rel_brg -= 360; + SG_NORMALIZE_RANGE(rel_brg, -180.0, 180.0); return rel_brg; } @@ -1148,80 +1147,7 @@ void FGAIBallistic::setTgtOffsets(double dt, double coeff){ _z_offset = (_tgt_z_offset * c) + (_z_offset * (1 - c)); } -void FGAIBallistic::formateToAC(double dt){ - - double hdg, pch, rll, agl, ht = 0; - - setTgtOffsets(dt, 25); - - if (_pnode != 0) { - setParentPos(); - hdg = _p_hdg_node->getDoubleValue(); - pch = _p_pch_node->getDoubleValue(); - rll = _p_rll_node->getDoubleValue(); - agl = _p_agl_node->getDoubleValue(); - ht = _p_alt_node->getDoubleValue(); - setOffsetPos(_parentpos, hdg, pch, rll); - setSpeed(_p_spd_node->getDoubleValue()); - }else { - hdg = manager->get_user_heading(); - pch = manager->get_user_pitch(); - rll = manager->get_user_roll(); - agl = manager->get_user_agl(); - ht = manager->get_user_altitude(); - setOffsetPos(userpos, hdg, pch, rll); - setSpeed(manager->get_user_speed()); - } - - // elapsed time has a random initialisation so that each - // wingman moves differently - _elapsed_time += dt; - // we derive a sine based factor to give us smoothly - // varying error between -1 and 1 - double factor = sin(SGMiscd::deg2rad(_elapsed_time * 10)); - double r_angle = 5 * factor; - double p_angle = 2.5 * factor; - double h_angle = 5 * factor; - double h_feet = 3 * factor; - - pos.setLatitudeDeg(_offsetpos.getLatitudeDeg()); - pos.setLongitudeDeg(_offsetpos.getLongitudeDeg()); - - if(agl <= 10) { - _height = ht; - //cout << "ht case1" << endl; - } else if (agl > 10 && agl <= 150 ) { - setHt(ht, dt, 1.0); - //cout << "ht case2" << endl; - } else if (agl > 150 && agl <= 250) { - setHt(_offsetpos.getElevationFt()+ h_feet, dt, 0.75); - //cout << "ht case3" << endl; - } else{ - setHt(_offsetpos.getElevationFt()+ h_feet, dt, 0.5); - //cout << "ht case4" << endl; - } - - pos.setElevationFt(_height); - - // these calculations are unreliable at slow speeds - if(speed >= 10) { - setHdg(_azimuth + h_angle, dt, 0.9); - setPch(_elevation + p_angle + _pitch_offset, dt, 0.9); - - if (roll <= 115 && roll >= -115) - setBnk(manager->get_user_roll() + r_angle + _roll_offset, dt, 0.5); - else - roll = manager->get_user_roll() + r_angle + _roll_offset; - - } else { - setHdg(manager->get_user_heading(), dt, 0.9); - setPch(manager->get_user_pitch() + _pitch_offset, dt, 0.9); - setBnk(manager->get_user_roll() + _roll_offset, dt, 0.9); - } - - setOffsetVelocity(dt, pos); -} void FGAIBallistic::calcVSHS(){ // calculate vertical and horizontal speed components double speed_fps = speed * SG_KT_TO_FPS; @@ -1263,7 +1189,7 @@ SGVec3d FGAIBallistic::getCartOffsetPos(SGGeod inpos, double user_heading, -_z_offset * SG_FEET_TO_METER); // Transform the user position to the horizontal local coordinate system. - SGQuatd hlTrans = SGQuatd::fromLonLat(userpos); + SGQuatd hlTrans = SGQuatd::fromLonLat(inpos); // and postrotate the orientation of the user model wrt the horizontal // local frame diff --git a/src/AIModel/AIBallistic.hxx b/src/AIModel/AIBallistic.hxx index 843c3db6b..b2b140c17 100644 --- a/src/AIModel/AIBallistic.hxx +++ b/src/AIModel/AIBallistic.hxx @@ -88,24 +88,30 @@ public: void setSlaved(bool s); void setSlavedLoad(bool s); void setPch (double e, double dt, double c); - void setHdg (double az, double dt, double c); + int setHdg (double az, double dt, double c); void setBnk(double r, double dt, double c); void setHt(double h, double dt, double c); - void setFormate(bool f); + void setSpd(double s, double dt, double c); void setParentNodes(const SGPropertyNode_ptr); void setParentPos(); + void setOffsetPos(SGGeod pos, double heading, double pitch, double roll); + void setOffsetVelocity(double dt, SGGeod pos); + double _getTime() const; double getRelBrgHitchToUser() const; double getElevHitchToUser() const; double getLoadOffset() const; double getContents(); + double getDistanceToHitch() const; + double getElevToHitch() const; + double getBearingToHitch() const; SGVec3d getCartHitchPos() const; bool getHtAGL(double start); bool getSlaved() const; - bool getFormate() const; +// bool getFormate() const; bool getSlavedLoad() const; virtual const char* getTypeString(void) const { return "ballistic"; } @@ -116,15 +122,32 @@ public: SGPropertyNode_ptr _force_azimuth_node; SGPropertyNode_ptr _force_elevation_node; + SGPropertyNode_ptr _pnode; // node for parent model + SGPropertyNode_ptr _p_pos_node; // nodes for parent parameters + SGPropertyNode_ptr _p_lat_node; + SGPropertyNode_ptr _p_lon_node; + SGPropertyNode_ptr _p_alt_node; + SGPropertyNode_ptr _p_agl_node; + SGPropertyNode_ptr _p_ori_node; + SGPropertyNode_ptr _p_pch_node; + SGPropertyNode_ptr _p_rll_node; + SGPropertyNode_ptr _p_hdg_node; + SGPropertyNode_ptr _p_vel_node; + SGPropertyNode_ptr _p_spd_node; + double _height; + double _speed; double _ht_agl_ft; // height above ground level double _azimuth; // degrees true double _elevation; // degrees double _rotation; // degrees double _speed_north_fps; double _speed_east_fps; + double _wind_from_east; // fps + double _wind_from_north; // fps + + double hs; - bool _formate_to_ac; void setTgtXOffset(double x); void setTgtYOffset(double y); @@ -140,6 +163,11 @@ public: double _tgt_z_offset; double _elapsed_time; + SGGeod _parentpos; + SGGeod _oldpos; + SGGeod _offsetpos; + SGGeod _oldoffsetpos; + private: virtual void reinit() { init(); } @@ -149,8 +177,6 @@ private: double _life_timer; // seconds double _gravity; // fps^2 double _buoyancy; // fps^2 - double _wind_from_east; // fps - double _wind_from_north; // fps bool _wind; // if true, local wind will be applied to object double _Cd; // drag coefficient double _mass; // slugs @@ -174,18 +200,18 @@ private: SGPropertyNode_ptr _impact_report_node; // report node for impact and collision SGPropertyNode_ptr _contents_node; // node for droptank etc. contents - SGPropertyNode_ptr _pnode; // node for parent model - SGPropertyNode_ptr _p_pos_node; // nodes for parent parameters - SGPropertyNode_ptr _p_lat_node; - SGPropertyNode_ptr _p_lon_node; - SGPropertyNode_ptr _p_alt_node; - SGPropertyNode_ptr _p_agl_node; - SGPropertyNode_ptr _p_ori_node; - SGPropertyNode_ptr _p_pch_node; - SGPropertyNode_ptr _p_rll_node; - SGPropertyNode_ptr _p_hdg_node; - SGPropertyNode_ptr _p_vel_node; - SGPropertyNode_ptr _p_spd_node; + //SGPropertyNode_ptr _pnode; // node for parent model + //SGPropertyNode_ptr _p_pos_node; // nodes for parent parameters + //SGPropertyNode_ptr _p_lat_node; + //SGPropertyNode_ptr _p_lon_node; + //SGPropertyNode_ptr _p_alt_node; + //SGPropertyNode_ptr _p_agl_node; + //SGPropertyNode_ptr _p_ori_node; + //SGPropertyNode_ptr _p_pch_node; + //SGPropertyNode_ptr _p_rll_node; + //SGPropertyNode_ptr _p_hdg_node; + //SGPropertyNode_ptr _p_vel_node; + //SGPropertyNode_ptr _p_spd_node; double _fuse_range; double _distance; @@ -204,22 +230,20 @@ private: void report_impact(double elevation, const FGAIBase *target = 0); void slaveToAC(double dt); void setContents(double c); - void formateToAC(double dt); void calcVSHS(); void calcNE(); - void setOffsetPos(SGGeod pos, double heading, double pitch, double roll); - void setOffsetVelocity(double dt, SGGeod pos); + //void setOffsetPos(SGGeod pos, double heading, double pitch, double roll); + //void setOffsetVelocity(double dt, SGGeod pos); SGVec3d getCartUserPos() const; SGVec3d getCartOffsetPos(SGGeod pos, double heading, double pitch, double roll) const; - double getDistanceLoadToHitch() const; - double getElevLoadToHitch() const; - double getBearingLoadToHitch() const; + //double getDistanceLoadToHitch() const; + //double getElevLoadToHitch() const; + //double getBearingLoadToHitch() const; double getRecip(double az); double getMass() const; - double hs; double _ground_offset; double _load_offset; double _old_height; @@ -227,10 +251,10 @@ private: SGVec3d _oldcartoffsetPos; SGVec3d _oldcartPos; - SGGeod _parentpos; - SGGeod _oldpos; - SGGeod _offsetpos; - SGGeod _oldoffsetpos; + //SGGeod _parentpos; + //SGGeod _oldpos; + //SGGeod _offsetpos; + //SGGeod _oldoffsetpos; }; diff --git a/src/AIModel/AIBase.cxx b/src/AIModel/AIBase.cxx index 501bb32cb..28c2d3892 100644 --- a/src/AIModel/AIBase.cxx +++ b/src/AIModel/AIBase.cxx @@ -65,6 +65,7 @@ FGAIBase::FGAIBase(object_type ot) : _impact_pitch(0), _impact_roll(0), _impact_speed(0), + _max_speed(300), _refID( _newAIModelID() ), _otype(ot), diff --git a/src/AIModel/AIBase.hxx b/src/AIModel/AIBase.hxx index fbaccd38f..80a93c206 100644 --- a/src/AIModel/AIBase.hxx +++ b/src/AIModel/AIBase.hxx @@ -29,6 +29,10 @@ #include #include #include +#include + +#include + #include
@@ -88,6 +92,14 @@ public: void setImpactElev( double e ); void setParentName(const string& p); void setName(const string& n); + void setMaxSpeed(double kts); + + void calcRangeBearing(double lat, double lon, double lat2, double lon2, + double &range, double &bearing) const; + double calcRelBearingDeg(double bearing, double heading); + double calcTrueBearingDeg(double bearing, double heading); + double calcRecipBearingDeg(double bearing); + bool setParentNode(); int getID() const; @@ -116,6 +128,8 @@ public: double _roll_offset; double _yaw_offset; + double _max_speed; + string _path; string _callsign; string _submodel; @@ -387,4 +401,35 @@ inline bool FGAIBase::getDie() { return delete_me; } inline FGAIBase::object_type FGAIBase::getType() { return _otype; } +inline void FGAIBase::calcRangeBearing(double lat, double lon, double lat2, double lon2, + double &range, double &bearing) const +{ + // calculate the bearing and range of the second pos from the first + double az2, distance; + geo_inverse_wgs_84(lat, lon, lat2, lon2, &bearing, &az2, &distance); + range = distance * SG_METER_TO_NM; +} + +inline double FGAIBase::calcRelBearingDeg(double bearing, double heading){ + double angle = bearing - heading; + SG_NORMALIZE_RANGE(angle, -180.0, 180.0); + return angle; +} + +inline double FGAIBase::calcTrueBearingDeg(double bearing, double heading){ + double angle = bearing + heading; + SG_NORMALIZE_RANGE(angle, 0.0, 360.0); + return angle; +} + +inline double FGAIBase::calcRecipBearingDeg(double bearing){ + double angle = bearing - 180; + SG_NORMALIZE_RANGE(angle, 0.0, 360.0); + return angle; +} + +inline void FGAIBase::setMaxSpeed(double m) { + _max_speed = m; +} + #endif // _FG_AIBASE_HXX diff --git a/src/AIModel/AIEscort.cxx b/src/AIModel/AIEscort.cxx index 255524cc7..5b429dfa3 100644 --- a/src/AIModel/AIEscort.cxx +++ b/src/AIModel/AIEscort.cxx @@ -269,36 +269,6 @@ void FGAIEscort::setParent() } -void FGAIEscort::calcRangeBearing(double lat, double lon, double lat2, double lon2, - double &range, double &bearing) const -{ - // calculate the bearing and range of the second pos from the first - double az2, distance; - geo_inverse_wgs_84(lat, lon, lat2, lon2, &bearing, &az2, &distance); - range = distance * SG_METER_TO_NM; -} - -double FGAIEscort::calcRelBearingDeg(double bearing, double heading) -{ - double angle = bearing - heading; - SG_NORMALIZE_RANGE(angle, -180.0, 180.0); - return angle; -} - -double FGAIEscort::calcTrueBearingDeg(double bearing, double heading) -{ - double angle = bearing + heading; - SG_NORMALIZE_RANGE(angle, 0.0, 360.0); - return angle; -} - -double FGAIEscort::calcRecipBearingDeg(double bearing) -{ - double angle = bearing - 180; - SG_NORMALIZE_RANGE(angle, 0.0, 360.0); - return angle; -} - SGVec3d FGAIEscort::getCartHitchPosAt(const SGVec3d& _off) const { double hdg = _selected_ac->getDoubleValue("orientation/true-heading-deg"); double pitch = _selected_ac->getDoubleValue("orientation/pitch-deg"); diff --git a/src/AIModel/AIEscort.hxx b/src/AIModel/AIEscort.hxx index e492d830e..d2e21f5ae 100644 --- a/src/AIModel/AIEscort.hxx +++ b/src/AIModel/AIEscort.hxx @@ -70,11 +70,11 @@ private: SGVec3d getCartHitchPosAt(const SGVec3d& off) const; - void calcRangeBearing(double lat, double lon, double lat2, double lon2, - double &range, double &bearing) const; - double calcRelBearingDeg(double bearing, double heading); - double calcTrueBearingDeg(double bearing, double heading); - double calcRecipBearingDeg(double bearing); +// void calcRangeBearing(double lat, double lon, double lat2, double lon2, +// double &range, double &bearing) const; + //double calcRelBearingDeg(double bearing, double heading); + //double calcTrueBearingDeg(double bearing, double heading); + //double calcRecipBearingDeg(double bearing); SGGeod _selectedpos; SGGeod _tgtpos; diff --git a/src/AIModel/AIManager.cxx b/src/AIModel/AIManager.cxx index e54a5bcee..419098112 100644 --- a/src/AIModel/AIManager.cxx +++ b/src/AIModel/AIManager.cxx @@ -240,6 +240,7 @@ FGAIManager::getNumAiObjects(void) const void FGAIManager::fetchUserState( void ) { + user_latitude = user_latitude_node->getDoubleValue(); user_longitude = user_longitude_node->getDoubleValue(); user_altitude = user_altitude_node->getDoubleValue(); diff --git a/src/AIModel/AIManager.hxx b/src/AIModel/AIManager.hxx index 891d3a506..89621689c 100644 --- a/src/AIModel/AIManager.hxx +++ b/src/AIModel/AIManager.hxx @@ -81,16 +81,16 @@ public: inline double get_wind_from_east() const {return wind_from_east; } inline double get_wind_from_north() const {return wind_from_north; } inline double get_user_roll() const { return user_roll; } - inline double get_user_agl() const { return user_agl; } + inline double get_user_agl() const { return user_altitude_agl; } int getNumAiObjects(void) const; void processScenario( const string &filename ); - static SGPropertyNode_ptr loadScenarioFile(const std::string& filename); + static SGPropertyNode_ptr loadScenarioFile(const std::string& filename); - static bool getStartPosition(const string& id, const string& pid, - SGGeod& geodPos, double& hdng, SGVec3d& uvw); + static bool getStartPosition(const string& id, const string& pid, + SGGeod& geodPos, double& hdng, SGVec3d& uvw); private: @@ -123,7 +123,6 @@ private: double user_yaw; double user_roll; double user_speed; - double user_agl; double wind_from_east; double wind_from_north; double _dt; diff --git a/src/AIModel/AIWingman.cxx b/src/AIModel/AIWingman.cxx index 87e0053e0..e3357b85f 100644 --- a/src/AIModel/AIWingman.cxx +++ b/src/AIModel/AIWingman.cxx @@ -21,12 +21,24 @@ # include #endif +#include +#include + + #include "AIWingman.hxx" -FGAIWingman::FGAIWingman() : FGAIBallistic(otWingman) +FGAIWingman::FGAIWingman() : FGAIBallistic(otWingman), +_formate_to_ac(true), +_break_angle(-90), +_break(false), +_join(false), +_coeff_hdg(5.0), +_coeff_pch(5.0) + { invisible = false; - _formate_to_ac = true; + _parent=""; + tgt_heading = 250; } @@ -54,11 +66,20 @@ void FGAIWingman::readFromScenario(SGPropertyNode* scFileNode) { setYawoffset(scFileNode->getDoubleValue("yaw-offset", 0.0)); setGroundOffset(scFileNode->getDoubleValue("ground-offset", 0.0)); setFormate(scFileNode->getBoolValue("formate", true)); + setMaxSpeed(scFileNode->getDoubleValue("max-speed-kts", 300.0)); + setCoeffHdg(scFileNode->getDoubleValue("coefficients/heading", 5.0)); + setCoeffPch(scFileNode->getDoubleValue("coefficients/pitch", 5.0)); + setCoeffBnk(scFileNode->getDoubleValue("coefficients/bank", 4.0)); + setCoeffSpd(scFileNode->getDoubleValue("coefficients/speed", 2.0)); + + } void FGAIWingman::bind() { FGAIBallistic::bind(); + props->untie("controls/slave-to-ac"); + props->tie("id", SGRawValueMethods(*this, &FGAIBase::getID)); props->tie("subID", SGRawValueMethods(*this, @@ -76,9 +97,30 @@ void FGAIWingman::bind() { &FGAIBase::_getLongitude, &FGAIBase::_setLongitude)); + props->tie("controls/break", SGRawValuePointer(&_break)); + props->tie("controls/join", SGRawValuePointer(&_join)); + props->tie("controls/formate-to-ac", - SGRawValueMethods - (*this, &FGAIBallistic::getFormate, &FGAIBallistic::setFormate)); + SGRawValueMethods + (*this, &FGAIWingman::getFormate, &FGAIWingman::setFormate)); + props->tie("controls/tgt-heading-deg", + SGRawValueMethods + (*this, &FGAIWingman::getTgtHdg, &FGAIWingman::setTgtHdg)); + props->tie("controls/tgt-speed-kt", + SGRawValueMethods + (*this, &FGAIWingman::getTgtSpd, &FGAIWingman::setTgtSpd)); + props->tie("controls/break-deg-rel", + SGRawValueMethods + (*this, &FGAIWingman::getBrkAng, &FGAIWingman::setBrkAng)); + props->tie("controls/coefficients/heading", + SGRawValuePointer(&_coeff_hdg)); + props->tie("controls/coefficients/pitch", + SGRawValuePointer(&_coeff_pch)); + props->tie("controls/coefficients/bank", + SGRawValuePointer(&_coeff_bnk)); + props->tie("controls/coefficients/speed", + SGRawValuePointer(&_coeff_spd)); + props->tie("orientation/pitch-deg", SGRawValuePointer(&pitch)); @@ -129,6 +171,15 @@ void FGAIWingman::unbind() { props->untie("orientation/true-heading-deg"); props->untie("controls/formate-to-ac"); + props->untie("controls/tgt-heading-deg"); + props->untie("controls/tgt-speed-kt"); + props->untie("controls/break-deg-rel"); + props->untie("controls/break"); + props->untie("controls/join"); + props->untie("controls/coefficients/heading"); + props->untie("controls/coefficients/pitch"); + props->untie("controls/coefficients/bank"); + props->untie("controls/coefficients/speed"); props->untie("submodels/serviceable"); @@ -167,11 +218,358 @@ bool FGAIWingman::init(bool search_in_AI_path) { _ht_agl_ft = 1e10; props->setStringValue("submodels/path", _path.c_str()); + + if(_parent != ""){ + setParentNode(); + } + + setParentNodes(_selected_ac); return true; } void FGAIWingman::update(double dt) { - FGAIBallistic::update(dt); +// FGAIBallistic::update(dt); + + if (_formate_to_ac){ + formateToAC(dt); + Transform(); + setBrkHdg(_break_angle); + }else if (_break) { + FGAIBase::update(dt); + tgt_altitude_ft = altitude_ft; + tgt_speed = speed; + tgt_roll = roll; + tgt_pitch = pitch; + Break(dt); + Transform(); + } else { + Join(dt); + Transform(); + } + +} + +double FGAIWingman::calcDistanceM(SGGeod pos1, SGGeod pos2) const { + //calculate the distance load to hitch + SGVec3d cartPos1 = SGVec3d::fromGeod(pos1); + SGVec3d cartPos2 = SGVec3d::fromGeod(pos2); + + SGVec3d diff = cartPos1 - cartPos2; + double distance = norm(diff); + return distance; +} + +double FGAIWingman::calcAngle(double range, SGGeod pos1, SGGeod pos2){ + + double angle = 0; + double distance = calcDistanceM(pos1, pos2); + double daltM = pos1.getElevationM() - pos2.getElevationM(); + + if (fabs(distance) < SGLimits::min()) { + angle = 0; + } else { + double sAngle = daltM/range; + sAngle = SGMiscd::min(1, SGMiscd::max(-1, sAngle)); + angle = SGMiscd::rad2deg(asin(sAngle)); + } + + return angle; } +void FGAIWingman::formateToAC(double dt){ + + double p_hdg, p_pch, p_rll, p_agl, p_ht = 0; + + setTgtOffsets(dt, 25); + + if (_pnode != 0) { + setParentPos(); + p_hdg = _p_hdg_node->getDoubleValue(); + p_pch = _p_pch_node->getDoubleValue(); + p_rll = _p_rll_node->getDoubleValue(); + //agl = _p_agl_node->getDoubleValue(); + p_ht = _p_alt_node->getDoubleValue(); + setOffsetPos(_parentpos, p_hdg, p_pch, p_rll); + setSpeed(_p_spd_node->getDoubleValue()); + }else { + _setUserPos(); + p_hdg = manager->get_user_heading(); + p_pch = manager->get_user_pitch(); + p_rll = manager->get_user_roll(); + //agl = manager->get_user_agl(); + p_ht = manager->get_user_altitude(); + setOffsetPos(userpos, p_hdg,p_pch, p_rll); + setSpeed(manager->get_user_speed()); + } + + // elapsed time has a random initialisation so that each + // wingman moves differently + _elapsed_time += dt; + + // we derive a sine based factor to give us smoothly + // varying error between -1 and 1 + double factor = sin(SGMiscd::deg2rad(_elapsed_time * 10)); + double r_angle = 5 * factor; + double p_angle = 2.5 * factor; + double h_angle = 5 * factor; + double h_feet = 3 * factor; + + p_agl = manager->get_user_agl(); + + if(p_agl <= 10) { + _height = p_ht; + //cout << "ht case1 " ; + } else if (p_agl > 10 && p_agl <= 150 ) { + setHt(p_ht, dt, 1.0); + //cout << "ht case2 " ; + } else if (p_agl > 150 && p_agl <= 250) { + setHt(_offsetpos.getElevationFt()+ h_feet, dt, 0.75); + //cout << "ht case3 " ; + } else{ + setHt(_offsetpos.getElevationFt()+ h_feet, dt, 0.5); + //cout << "ht case4 " ; + } + + pos.setElevationFt(_height); + pos.setLatitudeDeg(_offsetpos.getLatitudeDeg()); + pos.setLongitudeDeg(_offsetpos.getLongitudeDeg()); + + // these calculations are unreliable at slow speeds + if(speed >= 10) { + setHdg(p_hdg + h_angle, dt, 0.9); + setPch(p_pch + p_angle + _pitch_offset, dt, 0.9); + + if (roll <= 115 && roll >= -115) + setBnk(p_rll + r_angle + _roll_offset, dt, 0.5); + else + roll = p_rll + r_angle + _roll_offset; + + } else { + setHdg(p_hdg, dt, 0.9); + setPch(p_pch + _pitch_offset, dt, 0.9); + setBnk(p_rll + _roll_offset, dt, 0.9); + } + + setOffsetVelocity(dt, pos); +}// end formateToAC + +void FGAIWingman::Break(double dt) { + + Run(dt); + + //calculate the turn direction: 1 = right, -1 = left + double rel_brg = calcRelBearingDeg(tgt_heading, hdg); + int turn = SGMiscd::sign(rel_brg); + + // set heading and pitch + setHdg(tgt_heading, dt, _coeff_hdg); + setPch(0, dt, _coeff_pch); + + if (fabs(tgt_heading - hdg) >= 10) + setBnk(45 * turn , dt, _coeff_bnk); + else + setBnk(0, dt, _coeff_bnk); + +} // end Break + +void FGAIWingman::Join(double dt) { + + double range, bearing, az2; + double parent_hdg, parent_spd, parent_ht= 0; + double p_hdg, p_pch, p_rll = 0; + + setTgtOffsets(dt, 25); + + if (_pnode != 0) { + setParentPos(); + p_hdg = _p_hdg_node->getDoubleValue(); + p_pch = _p_pch_node->getDoubleValue(); + p_rll = _p_rll_node->getDoubleValue(); + setOffsetPos(_parentpos, p_hdg, p_pch, p_rll); + parent_hdg = _p_hdg_node->getDoubleValue(); + parent_spd = _p_spd_node->getDoubleValue(); + }else { + _setUserPos(); + p_hdg = manager->get_user_heading(); + p_pch = manager->get_user_pitch(); + p_rll = manager->get_user_roll(); + setOffsetPos(userpos, p_hdg, p_pch, p_rll); + parent_hdg = manager->get_user_heading(); + parent_spd = manager->get_user_speed(); + } + + setSpeed(parent_spd); + + double distance = calcDistanceM(pos, _offsetpos); + double daltM = _offsetpos.getElevationM() - pos.getElevationM(); + double limit = 10; + double hdg_l_lim = parent_hdg - limit; + SG_NORMALIZE_RANGE(hdg_l_lim, 0.0, 360.0); + double hdg_r_lim = parent_hdg + limit; + SG_NORMALIZE_RANGE(hdg_r_lim, 0.0, 360.0); + + if (distance <= 2 && fabs(daltM) <= 2 && + (hdg >= hdg_l_lim || hdg <= hdg_r_lim)){ + _height = _offsetpos.getElevationFt(); + _formate_to_ac = true; + _join = false; + + SG_LOG(SG_GENERAL, SG_ALERT, _name << " joined " << " RANGE " << distance + << " SPEED " << speed ); + + return; + } + + geo_inverse_wgs_84(pos, _offsetpos, &bearing, &az2, &range); + + double rel_brg = calcRelBearingDeg(bearing, hdg); + double recip_brg = calcRecipBearingDeg(bearing); + double angle = calcAngle(distance,_offsetpos, pos); + double approx_angle = atan2(daltM, range); + double frm_spd = 50; // formation speed + double join_rnge = 1000.0; + double recip_parent_hdg = calcRecipBearingDeg(parent_hdg); + int turn = SGMiscd::sign(rel_brg);// turn direction: 1 = right, -1 = left + + if (range <= join_rnge && (hdg >= hdg_l_lim || hdg <= hdg_r_lim)){ + + //these are the rules governing joining + + if ((rel_brg <= -175 || rel_brg >= 175) && range <=10 ){ + // station is behind us - back up a bit + setSpeed(parent_spd - ((frm_spd/join_rnge) * range)); + setHdg(recip_brg, dt, _coeff_hdg); + setPch(angle, dt, _coeff_pch); + //cout << _name << " backing up HEADING " << hdg + // << " RANGE " << range; + } else if (rel_brg >= -5 || rel_brg <= 5) { + // station is in front of us - slow down + setSpeed(parent_spd + ((frm_spd/100) * range)); + //SGMiscd::clip + setHdg(bearing, dt, 1.5); + setPch(angle, dt, _coeff_pch); + //cout << _name << " slowing HEADING " << hdg + // << " RANGE " << range < join_rnge && range <= 2000 ){ + //approach phase + //cout << _name << " approach HEADING " << hdg + // << " RANGE " << range<< endl; + setSpd(parent_spd + frm_spd, dt, 2.0); + setSpeed(_speed); + setHdg(bearing, dt, _coeff_hdg); + setPch(angle, dt, _coeff_pch); + } else { + //hurry up + //cout << _name << " hurry up HEADING " << hdg + // << " RANGE " << range<< endl; + setSpd(_max_speed -10, dt, 2.0); + setSpeed(_speed); + setHdg(bearing, dt, _coeff_hdg); + setPch(angle, dt, _coeff_pch); + } + + Run(dt); + + // set roll + + if (fabs(bearing - hdg) >= 10) + setBnk(45 * turn , dt, _coeff_bnk); + else + setBnk(0, dt, _coeff_bnk); + +} // end Join + +void FGAIWingman::Run(double dt) { + + // don't let speed become negative + SG_CLAMP_RANGE(speed, 100.0, _max_speed); + + double speed_fps = speed * SG_KT_TO_FPS; + + // calculate vertical and horizontal speed components + if (speed == 0.0) { + hs = vs = 0.0; + } else { + vs = sin( pitch * SG_DEGREES_TO_RADIANS ) * speed_fps; + hs = cos( pitch * SG_DEGREES_TO_RADIANS ) * speed_fps; + } + + //cout << "vs hs " << vs << " " << hs << endl; + + //resolve horizontal speed into north and east components: + double speed_north_fps = cos(hdg / SG_RADIANS_TO_DEGREES) * hs; + double speed_east_fps = sin(hdg / SG_RADIANS_TO_DEGREES) * hs; + + // convert horizontal speed (fps) to degrees per second + double speed_north_deg_sec = speed_north_fps / ft_per_deg_lat; + double speed_east_deg_sec = speed_east_fps / ft_per_deg_lon; + + //get wind components + _wind_from_north = manager->get_wind_from_north(); + _wind_from_east = manager->get_wind_from_east(); + + // convert wind speed (fps) to degrees lat/lon per second + double wind_speed_from_north_deg_sec = _wind_from_north / ft_per_deg_lat; + double wind_speed_from_east_deg_sec = _wind_from_east / ft_per_deg_lon; + + //recombine the horizontal velocity components + hs = sqrt(((speed_north_fps) * (speed_north_fps)) + + ((speed_east_fps)* (speed_east_fps ))); + + if (hs <= 0.00001) + hs = 0; + + if (vs <= 0.00001 && vs >= -0.00001) + vs = 0; + + //cout << "lat " << pos.getLatitudeDeg()<< endl; + // set new position + pos.setLatitudeDeg( pos.getLatitudeDeg() + + (speed_north_deg_sec - wind_speed_from_north_deg_sec) * dt ); + pos.setLongitudeDeg( pos.getLongitudeDeg() + + (speed_east_deg_sec - wind_speed_from_east_deg_sec ) * dt ); + pos.setElevationFt(pos.getElevationFt() + vs * dt); + + //cout << _name << " run hs " << hs << " vs " << vs << endl; + + // recalculate total speed + if ( vs == 0 && hs == 0) + speed = 0; + else + speed = sqrt( vs * vs + hs * hs) / SG_KT_TO_FPS; + + // recalculate elevation and azimuth (velocity vectors) + pitch = atan2( vs, hs ) * SG_RADIANS_TO_DEGREES; + hdg = atan2((speed_east_fps),(speed_north_fps))* SG_RADIANS_TO_DEGREES; + + // rationalise heading + SG_NORMALIZE_RANGE(hdg, 0.0, 360.0); + +}// end Run + // end AIWingman diff --git a/src/AIModel/AIWingman.hxx b/src/AIModel/AIWingman.hxx index c61de7f34..038df5d08 100644 --- a/src/AIModel/AIWingman.hxx +++ b/src/AIModel/AIWingman.hxx @@ -25,6 +25,10 @@ #include "AIManager.hxx" #include "AIBase.hxx" +#include +#include + + class FGAIWingman : public FGAIBallistic { public: FGAIWingman(); @@ -42,6 +46,106 @@ private: virtual void reinit() { init(); } virtual void update (double dt); + void formateToAC(double dt); + void Break(double dt); + void Join(double dt); + void Run(double dt); + + double getDistanceToOffset() const; + double getElevToOffset() const; + + double calcAngle(double rangeM, SGGeod pos1, SGGeod pos2); + double calcDistanceM(SGGeod pos1, SGGeod pos2) const; + + bool _formate_to_ac; + bool _break; + bool _join; + + double _break_angle; //degrees relative + double _coeff_hdg; //dimensionless coefficient + double _coeff_pch; //dimensionless coefficient + double _coeff_bnk; //dimensionless coefficient + double _coeff_spd; //dimensionless coefficient + + + + inline void setFormate(bool f); + inline void setTgtHdg(double hdg); + inline void setTgtSpd(double spd); + inline void setBrkHdg(double angle); + inline void setBrkAng(double angle); + inline void setCoeffHdg(double h); + inline void setCoeffPch(double p); + inline void setCoeffBnk(double r); + inline void setCoeffSpd(double s); + + inline bool getFormate() const { return _formate_to_ac;} + + inline double getTgtHdg() const { return tgt_heading;} + inline double getTgtSpd() const { return tgt_speed;} + inline double getBrkAng() const { return _break_angle;} + + inline SGVec3d getCartInPos(SGGeod in_pos) const; + }; +void FGAIWingman::setFormate(bool f) { + _formate_to_ac = f; +} + +void FGAIWingman::setTgtHdg(double h) { + tgt_heading = h; +} + +void FGAIWingman::setTgtSpd(double s) { + tgt_speed = s; +} + +void FGAIWingman::setBrkHdg(double a){ + tgt_heading = hdg + a ; + SG_NORMALIZE_RANGE(tgt_heading, 0.0, 360.0); +} + +void FGAIWingman::setBrkAng(double a){ + _break_angle = a ; + SG_NORMALIZE_RANGE(_break_angle, -180.0, 180.0); +} + +void FGAIWingman::setCoeffHdg(double h){ + _coeff_hdg = h; +} + +void FGAIWingman::setCoeffPch(double p){ + _coeff_pch = p; +} + +void FGAIWingman::setCoeffBnk(double b){ + _coeff_bnk = b; +} + +void FGAIWingman::setCoeffSpd(double s){ + _coeff_spd = s; +} + +//bool FGAIWingman::getFormate() const { +// return _formate_to_ac; +//} + +//double FGAIWingman::getTgtHdg() const{ +// return tgt_heading; +//} + +//double FGAIWingman::getTgtSpd() const{ +// return tgt_speed; +//} + +//double FGAIWingman::getBrkAng() const{ +// return _break_angle; +//} + +SGVec3d FGAIWingman::getCartInPos(SGGeod in_pos) const { + SGVec3d cartPos = SGVec3d::fromGeod(in_pos); + return cartPos; +} + #endif // FG_AIWINGMAN_HXX -- 2.39.5